From 8ba4cb396ddee981724d362227db0f8b06b867a2 Mon Sep 17 00:00:00 2001 From: RandomityGuy <31925790+RandomityGuy@users.noreply.github.com> Date: Thu, 3 Jun 2021 16:35:59 +0530 Subject: [PATCH] implement gjk collision for dts --- compile.hxml | 1 - src/DifBuilder.hx | 16 ++- src/DtsObject.hx | 117 +++++++++++++--- src/InteriorGeometry.hx | 3 - src/Main.hx | 66 ++++----- src/MarbleWorld.hx | 4 + src/collision/CollisionEntity.hx | 14 +- src/collision/CollisionHull.hx | 57 ++++++++ src/collision/CollisionSurface.hx | 15 +++ src/collision/CollisionWorld.hx | 6 +- src/collision/gjk/ConvexHull.hx | 58 ++++++++ src/collision/gjk/GJK.hx | 215 ++++++++++++++++++++++++++++++ src/collision/gjk/Sphere.hx | 19 +++ 13 files changed, 528 insertions(+), 63 deletions(-) create mode 100644 src/collision/CollisionHull.hx create mode 100644 src/collision/gjk/ConvexHull.hx create mode 100644 src/collision/gjk/GJK.hx create mode 100644 src/collision/gjk/Sphere.hx diff --git a/compile.hxml b/compile.hxml index 209b8e69..2056d411 100644 --- a/compile.hxml +++ b/compile.hxml @@ -1,5 +1,4 @@ -cp src --lib headbutt -lib heaps -lib hlsdl -lib polygonal-ds diff --git a/src/DifBuilder.hx b/src/DifBuilder.hx index 8e21c8d5..6e61252b 100644 --- a/src/DifBuilder.hx +++ b/src/DifBuilder.hx @@ -1,5 +1,6 @@ package src; +import h3d.mat.Material; import src.ResourceLoader; import src.PathedInterior; import h3d.Vector; @@ -213,6 +214,9 @@ class DifBuilder { itr.collider = collider; function canFindTex(tex:String) { + if (["NULL"].contains(tex)) { + return false; + } if (tex.indexOf('/') != -1) { tex = tex.split('/')[1]; } @@ -289,9 +293,15 @@ class DifBuilder { prim.uvs = uvs; prim.normals = normals; - var texture:Texture = loader.load(tex(grp)).toImage().toTexture(); - texture.wrap = Wrap.Repeat; - var material = h3d.mat.Material.create(texture); + var material:Material; + var texture:Texture; + if (canFindTex(grp)) { + texture = loader.load(tex(grp)).toImage().toTexture(); + texture.wrap = Wrap.Repeat; + material = h3d.mat.Material.create(texture); + } else { + material = Material.create(); + } // material.mainPass.wireframe = true; var mesh = new Mesh(prim, material, itr); diff --git a/src/DtsObject.hx b/src/DtsObject.hx index 2cfe43ad..686e807d 100644 --- a/src/DtsObject.hx +++ b/src/DtsObject.hx @@ -1,5 +1,8 @@ package src; +import collision.CollisionHull; +import collision.CollisionSurface; +import collision.CollisionEntity; import hxd.FloatBuffer; import h3d.prim.DynamicPrimitive; import h3d.scene.Trail; @@ -76,6 +79,7 @@ class DtsObject extends Object { var fs:Loader; var rootObject:Object; + var colliders:Array; public function new() { super(); @@ -89,6 +93,7 @@ class DtsObject extends Object { var graphNodes = []; var rootNodesIdx = []; + colliders = []; for (i in 0...this.dts.nodes.length) { graphNodes.push(new Object()); @@ -105,7 +110,6 @@ class DtsObject extends Object { this.graphNodes = graphNodes; // this.rootGraphNodes = graphNodes.filter(node -> node.parent == null); - this.updateNodeTransforms(); var affectedBySequences = this.dts.sequences.length > 0 ? (this.dts.sequences[0].rotationMatters.length < 0 ? 0 : this.dts.sequences[0].rotationMatters[0]) | (this.dts.sequences[0].translationMatters.length > 0 ? this.dts.sequences[0].translationMatters[0] : 0) : 0; @@ -127,8 +131,8 @@ class DtsObject extends Object { if (mesh == null) continue; - var vertices = mesh.vertices.map(v -> new Vector(v.x, v.y, v.z)); - var vertexNormals = mesh.normals.map(v -> new Vector(v.x, v.y, v.z)); + var vertices = mesh.vertices.map(v -> new Vector(-v.x, v.y, v.z)); + var vertexNormals = mesh.normals.map(v -> new Vector(-v.x, v.y, v.z)); var geometry = this.generateMaterialGeometry(mesh, vertices, vertexNormals); for (k in 0...geometry.length) { @@ -145,6 +149,41 @@ class DtsObject extends Object { } } + for (i in 0...dts.nodes.length) { + var objects = dts.objects.filter(object -> object.node == i); + var meshSurfaces = []; + var collider = new CollisionHull(); + + for (object in objects) { + var isCollisionObject = dts.names[object.name].substr(0, 3).toLowerCase() == "col"; + + if (isCollisionObject) { + for (j in object.firstMesh...(object.firstMesh + object.numMeshes)) { + if (j >= this.dts.meshes.length) + continue; + + var mesh = this.dts.meshes[j]; + if (mesh == null) + continue; + + var vertices = mesh.vertices.map(v -> new Vector(v.x, v.y, v.z)); + var vertexNormals = mesh.normals.map(v -> new Vector(v.x, v.y, v.z)); + + var surfaces = this.generateCollisionGeometry(mesh, vertices, vertexNormals); + for (surface in surfaces) + collider.addSurface(surface); + meshSurfaces = meshSurfaces.concat(surfaces); + } + } + } + if (meshSurfaces.length != 0) + colliders.push(collider); + else + colliders.push(null); + } + + this.updateNodeTransforms(); + for (i in 0...this.dts.meshes.length) { var mesh = this.dts.meshes[i]; if (mesh == null) @@ -189,7 +228,7 @@ class DtsObject extends Object { rootObject.addChild(this.skinMeshData.geometry); } - rootObject.scaleX = -1; + // rootObject.scaleX = -1; } function computeMaterials() { @@ -256,9 +295,58 @@ class DtsObject extends Object { quat.toMatrix(mat); mat.setPosition(new Vector(translation.x, translation.y, translation.z)); this.graphNodes[i].setTransform(mat); + var absTform = this.graphNodes[i].getAbsPos().clone(); + if (this.colliders[i] != null) + // this.colliders[i].setTransform(Matrix.I()); + this.colliders[i].setTransform(absTform); } } + function generateCollisionGeometry(dtsMesh:dts.Mesh, vertices:Array, vertexNormals:Array) { + var surfaces = this.materials.map(x -> new CollisionSurface()); + for (surface in surfaces) { + surface.points = []; + surface.normals = []; + surface.indices = []; + } + for (primitive in dtsMesh.primitives) { + var k = 0; + var geometrydata = surfaces[primitive.matIndex]; + + for (i in primitive.firstElement...(primitive.firstElement + primitive.numElements - 2)) { + var i1 = dtsMesh.indices[i]; + var i2 = dtsMesh.indices[i + 1]; + var i3 = dtsMesh.indices[i + 2]; + + if (k % 2 == 0) { + // Swap the first and last index to mainting correct winding order + var temp = i1; + i1 = i3; + i3 = temp; + } + + for (index in [i1, i2, i3]) { + var vertex = vertices[index]; + geometrydata.points.push(new Vector(-vertex.x, vertex.y, vertex.z)); + + var normal = vertexNormals[index]; + geometrydata.normals.push(new Vector(-normal.x, normal.y, normal.z)); + } + + geometrydata.indices.push(geometrydata.indices.length); + geometrydata.indices.push(geometrydata.indices.length); + geometrydata.indices.push(geometrydata.indices.length); + + k++; + } + } + for (surface in surfaces) { + surface.generateBoundingBox(); + // surface.generateNormals(); + } + return surfaces; + } + function generateMaterialGeometry(dtsMesh:dts.Mesh, vertices:Array, vertexNormals:Array) { var materialGeometry:Array = this.materials.map(x -> { vertices: [], @@ -490,8 +578,6 @@ class DtsObject extends Object { if (prim.buffer != null) { prim.addNormals(); prim.flush(); - // prim.buffer.uploadBytes() - // prim.buffer.dispose(); } mesh.primitive = prim; mesh = cast info.geometry.children[meshIndex]; @@ -501,35 +587,28 @@ class DtsObject extends Object { vbuffer = prim.getBuffer(prim.points.length); } } - // if (prim.buffer == null) { var vertex = info.vertices[i]; var normal = info.normals[i]; prim.points[pos] = vertex.toPoint(); if (prim.buffer != null) { prim.dirtyFlags[pos] = true; } - // } else { - // vbuffer[pos * 8] = info.vertices[i].x; - // vbuffer[(pos * 8) + 1] = info.vertices[i].y; - // vbuffer[(pos * 8) + 2] = info.vertices[i].z; - // var fb = new FloatBuffer(); - // fb.push(info.vertices[i].x); - // fb.push(info.vertices[i].y); - // fb.push(info.vertices[i].z); - // prim.buffer.uploadVector(fb, pos * 8, 1); - // } - // prim.normals[pos] = normal.toPoint(); pos++; } if (prim.buffer != null) { prim.addNormals(); prim.flush(); - // prim.buffer.dispose(); } if (_regenNormals) { _regenNormals = false; } } + + for (i in 0...this.colliders.length) { + var absTform = this.graphNodes[i].getAbsPos().clone(); + if (this.colliders[i] != null) + this.colliders[i].setTransform(absTform); + } } public function getMountTransform(mountPoint:Int) { diff --git a/src/InteriorGeometry.hx b/src/InteriorGeometry.hx index 91d20a8e..63fb6ac4 100644 --- a/src/InteriorGeometry.hx +++ b/src/InteriorGeometry.hx @@ -2,9 +2,6 @@ package src; import h3d.Matrix; import collision.CollisionEntity; -import headbutt.threed.Headbutt; -import headbutt.threed.shapes.Sphere; -import glm.Vec3; import dif.math.Point3F; import h3d.scene.Mesh; import h3d.col.Bounds; diff --git a/src/Main.hx b/src/Main.hx index 1abcc83b..26e01d82 100644 --- a/src/Main.hx +++ b/src/Main.hx @@ -34,49 +34,53 @@ class Main extends hxd.App { var loader = new Loader(fileSystem); dtsObj = new DtsObject(); - dtsObj.dtsPath = "data/shapes/hazards/tornado.dts"; + dtsObj.dtsPath = "data/shapes/items/timetravel.dts"; dtsObj.isCollideable = false; dtsObj.isTSStatic = false; dtsObj.fs = loader; + // dtsObj.setRotation(Math.PI / 2, 0, 0); world = new MarbleWorld(s3d); var db = new InteriorGeometry(); DifBuilder.loadDif("data/interiors/beginner/training_friction.dif", loader, db); world.addInterior(db); + var tform = db.getTransform(); + tform.setPosition(new Vector(0, 0, 7)); + db.setTransform(tform); - var pi = new PathedInterior(); - DifBuilder.loadDif("data/interiors/addon/smallplatform.dif", loader, pi); - var pim = pi.getTransform(); - pim.setPosition(new Vector(5, 0, 0)); - pi.setTransform(pim); + // var pi = new PathedInterior(); + // DifBuilder.loadDif("data/interiors/addon/smallplatform.dif", loader, pi); + // var pim = pi.getTransform(); + // pim.setPosition(new Vector(5, 0, 0)); + // pi.setTransform(pim); - var cube = new Cube(); - cube.addUVs(); - cube.addNormals(); - var mat = Material.create(); + // var cube = new Cube(); + // cube.addUVs(); + // cube.addNormals(); + // var mat = Material.create(); - var m1 = new PathedInteriorMarker(); - m1.msToNext = 5; - m1.position = new Vector(0, 0, 0); - m1.smoothingType = ""; - m1.rotation = new Quat(); + // var m1 = new PathedInteriorMarker(); + // m1.msToNext = 5; + // m1.position = new Vector(0, 0, 0); + // m1.smoothingType = ""; + // m1.rotation = new Quat(); - var m2 = new PathedInteriorMarker(); - m2.msToNext = 3; - m2.position = new Vector(0, 0, 5); - m2.smoothingType = ""; - m2.rotation = new Quat(); + // var m2 = new PathedInteriorMarker(); + // m2.msToNext = 3; + // m2.position = new Vector(0, 0, 5); + // m2.smoothingType = ""; + // m2.rotation = new Quat(); - var m3 = new PathedInteriorMarker(); - m3.msToNext = 5; - m3.position = new Vector(0, 0, 0); - m3.smoothingType = ""; - m3.rotation = new Quat(); + // var m3 = new PathedInteriorMarker(); + // m3.msToNext = 5; + // m3.position = new Vector(0, 0, 0); + // m3.smoothingType = ""; + // m3.rotation = new Quat(); - pi.markerData = [m1, m2, m3]; + // pi.markerData = [m1, m2, m3]; - world.addPathedInterior(pi); + // world.addPathedInterior(pi); world.addDtsObject(dtsObj); @@ -103,11 +107,11 @@ class Main extends hxd.App { var marble = new Marble(); marble.controllable = true; world.addMarble(marble); - marble.setPosition(0, 0, 2); + marble.setPosition(5, 0, 5); - var marble2 = new Marble(); - world.addMarble(marble2); - marble2.setPosition(5, 0, 5); + // var marble2 = new Marble(); + // world.addMarble(marble2); + // marble2.setPosition(5, 0, 5); // marble.setPosition(-10, -5, 5); } diff --git a/src/MarbleWorld.hx b/src/MarbleWorld.hx index 8568c098..1c9a1ede 100644 --- a/src/MarbleWorld.hx +++ b/src/MarbleWorld.hx @@ -44,6 +44,10 @@ class MarbleWorld { this.dtsObjects.push(obj); this.scene.addChild(obj); obj.init(); + for (collider in obj.colliders) { + if (collider != null) + this.collisionWorld.addEntity(collider); + } } public function addMarble(marble:Marble) { diff --git a/src/collision/CollisionEntity.hx b/src/collision/CollisionEntity.hx index 21c3e3aa..c13daa2f 100644 --- a/src/collision/CollisionEntity.hx +++ b/src/collision/CollisionEntity.hx @@ -76,6 +76,14 @@ class CollisionEntity implements IOctreeObject { var tform = transform.clone(); tform.setPosition(tform.getPosition().add(this.velocity.multiply(dt))); + function toDifPoint(pt:Vector) { + return new Point3F(pt.x, pt.y, pt.z); + } + + function fromDifPoint(pt:Point3F) { + return new Vector(pt.x, pt.y, pt.z); + } + var contacts = []; for (obj in surfaces) { @@ -91,8 +99,8 @@ class CollisionEntity implements IOctreeObject { var res = Collision.IntersectTriangleSphere(v0, v, v2, surfacenormal, position, radius); var closest = res.point; - // Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surface.normals[surface.indices[i]]); - if (res.result) { + // closest = Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surface.normals[surface.indices[i]]); + if (closest != null) { if (position.sub(closest).lengthSq() < radius * radius) { var normal = res.normal; @@ -100,7 +108,7 @@ class CollisionEntity implements IOctreeObject { normal.normalize(); var cinfo = new CollisionInfo(); - cinfo.normal = res.normal; // surface.normals[surface.indices[i]]; + cinfo.normal = normal; // surface.normals[surface.indices[i]]; cinfo.point = closest; // cinfo.collider = this; cinfo.velocity = this.velocity; diff --git a/src/collision/CollisionHull.hx b/src/collision/CollisionHull.hx new file mode 100644 index 00000000..fde28e31 --- /dev/null +++ b/src/collision/CollisionHull.hx @@ -0,0 +1,57 @@ +package collision; + +import h3d.col.Bounds; +import collision.gjk.GJK; +import collision.gjk.ConvexHull; +import h3d.Vector; + +class CollisionHull extends CollisionEntity { + var hull:ConvexHull; + + public function new() { + super(); + } + + public override function sphereIntersection(collisionEntity:SphereCollisionEntity, dt:Float):Array { + var bbox = this.boundingBox; + var box = new Bounds(); + var pos = collisionEntity.transform.getPosition(); + box.xMin = pos.x - collisionEntity.radius; + box.yMin = pos.y - collisionEntity.radius; + box.zMin = pos.z - collisionEntity.radius; + box.xMax = pos.x + collisionEntity.radius; + box.yMax = pos.y + collisionEntity.radius; + box.zMax = pos.z + collisionEntity.radius; + + if (bbox.collide(box)) { + var sph = new collision.gjk.Sphere(); + sph.position = pos; + sph.radius = collisionEntity.radius; + var newTform = this.transform.clone(); + var newpos = this.transform.getPosition().add(this.velocity.multiply(dt)); + newTform.setPosition(newpos); + hull.setTransform(newTform); + + var pt = GJK.gjk(sph, this.hull); + if (pt != null) { + var cinfo = new CollisionInfo(); + cinfo.normal = pt.normalized(); + cinfo.point = sph.position.sub(pt); + cinfo.velocity = velocity; + cinfo.contactDistance = sph.radius + pt.length(); + cinfo.restitution = 1; + cinfo.friction = 1; + cinfo.force = 0; + return [cinfo]; + } + } + return []; + } + + public override function addSurface(surface:CollisionSurface) { + super.addSurface(surface); + if (hull == null) + hull = new ConvexHull([]); + hull.vertices = hull.vertices.concat(surface.points); + } +} diff --git a/src/collision/CollisionSurface.hx b/src/collision/CollisionSurface.hx index b2b64ce1..28600b4c 100644 --- a/src/collision/CollisionSurface.hx +++ b/src/collision/CollisionSurface.hx @@ -24,6 +24,21 @@ class CollisionSurface implements IOctreeObject { return 2; } + public function generateNormals() { + var i = 0; + normals = [for (n in points) null]; + while (i < indices.length) { + var p1 = points[indices[i]]; + var p2 = points[indices[i + 1]]; + var p3 = points[indices[i + 2]]; + var n = p2.sub(p1).cross(p3.sub(p1)).normalized().multiply(-1); + normals[indices[i]] = n; + normals[indices[i + 1]] = n; + normals[indices[i + 2]] = n; + i += 3; + } + } + public function generateBoundingBox() { var boundingBox = new Bounds(); boundingBox.xMin = 10e8; diff --git a/src/collision/CollisionWorld.hx b/src/collision/CollisionWorld.hx index 0003d0b7..b298cca4 100644 --- a/src/collision/CollisionWorld.hx +++ b/src/collision/CollisionWorld.hx @@ -44,9 +44,9 @@ class CollisionWorld { box.xMin = center.x - radius; box.yMin = center.y - radius; box.zMin = center.z - radius; - box.xMax = center.x - radius; - box.yMax = center.y - radius; - box.zMax = center.z - radius; + box.xMax = center.x + radius; + box.yMax = center.y + radius; + box.zMax = center.z + radius; var contacts:Array = []; diff --git a/src/collision/gjk/ConvexHull.hx b/src/collision/gjk/ConvexHull.hx new file mode 100644 index 00000000..ee12efcd --- /dev/null +++ b/src/collision/gjk/ConvexHull.hx @@ -0,0 +1,58 @@ +package collision.gjk; + +import h3d.Vector; +import h3d.Matrix; + +class ConvexHull { + public var vertices:Array; + + public var transform:Matrix; + + public var centre(get, never):Vector; + + var _centercache:Vector; + + function get_centre():Vector { + if (_centercache != null) + return _centercache; + else { + var sum = new Vector(); + for (v in vertices) { + sum = sum.add(v.transformed(this.transform)); + } + sum = sum.multiply(1 / vertices.length); + _centercache = sum; + return _centercache; + } + } + + public function new(vertices:Array) { + this.transform = Matrix.I(); + this.vertices = vertices; + } + + public function setTransform(matrix:Matrix):Void { + if (this.transform != matrix) { + this.transform = matrix; + this._centercache = null; + } + } + + public function support(direction:Vector):Vector { + var furthestDistance:Float = Math.NEGATIVE_INFINITY; + var furthestVertex:Vector = new Vector(); + + for (v in vertices) { + var v2 = v.transformed(transform); + var distance:Float = v2.dot(direction); + if (distance > furthestDistance) { + furthestDistance = distance; + furthestVertex.x = v2.x; + furthestVertex.y = v2.y; + furthestVertex.z = v2.z; + } + } + + return furthestVertex; + } +} diff --git a/src/collision/gjk/GJK.hx b/src/collision/gjk/GJK.hx new file mode 100644 index 00000000..a6cda3ed --- /dev/null +++ b/src/collision/gjk/GJK.hx @@ -0,0 +1,215 @@ +package collision.gjk; + +import h3d.Vector; + +// Code taken from https://github.com/kevinmoran/GJK/blob/master/main.cpp +class GJK { + public static var maxIterations = 64; + public static var maxEpaFaces = 64; + public static var epaTolerance = 0.0001; + public static var maxEpaLooseEdges = 64; + public static var maxEpaIterations = 64; + + public static function gjk(sphere:Sphere, hull:ConvexHull) { + var searchDir = sphere.position.sub(hull.centre); + var a = new Vector(); + var b = new Vector(); + var c = new Vector(); + var d = new Vector(); + + c = hull.support(searchDir).sub(sphere.support(searchDir.multiply(-1))); + searchDir = c.multiply(-1); + b = hull.support(searchDir).sub(sphere.support(searchDir.multiply(-1))); + if (b.dot(searchDir) < 0) + return null; + + searchDir = c.sub(b).cross(b.multiply(-1)).cross(c.sub(b)); + if (searchDir.length() == 0) { + searchDir = c.sub(b).cross(new Vector(1, 0, 0)); + if (searchDir.length() == 0.00) + searchDir = c.sub(b).cross(new Vector(0, 0, -1)); + } + + var simpDim = 2; + + for (i in 0...maxIterations) { + a = hull.support(searchDir).sub(sphere.support(searchDir.multiply(-1))); + if (a.dot(searchDir) < 0) { + return null; + } + + simpDim++; + if (simpDim == 3) { + var n = b.sub(a).cross(c.sub(a)); + var ao = a.multiply(-1); + + simpDim = 2; + if (b.sub(a).cross(n).dot(ao) > 0) { + c = a; + searchDir = b.sub(a).cross(ao).cross(b.sub(a)); + } else if (n.cross(c.sub(a)).dot(ao) > 0) { + b = a; + searchDir = c.sub(a).cross(ao).cross(c.sub(a)); + } else { + simpDim = 3; + if (n.dot(ao) > 0) { + d = c; + c = b; + b = a; + searchDir = n; + } else { + d = b; + b = a; + searchDir = n.multiply(-1); + } + } + } else { + var abc = b.sub(a).cross(c.sub(a)); + var acd = c.sub(a).cross(d.sub(a)); + var adb = d.sub(a).cross(b.sub(a)); + var ao = a.multiply(-1); + simpDim = 3; + if (abc.dot(ao) > 0) { + d = c; + c = b; + b = a; + searchDir = abc; + } else if (acd.dot(ao) > 0) { + b = a; + searchDir = acd; + } else if (adb.dot(ao) > 0) { + c = d; + d = b; + b = a; + searchDir = adb; + } else { + return epa(a, b, c, d, sphere, hull); + } + } + } + return null; + } + + public static function epa(a:Vector, b:Vector, c:Vector, d:Vector, sphere:Sphere, hull:ConvexHull) { + var faces = []; + for (i in 0...maxEpaFaces) + faces.push([new Vector(), new Vector(), new Vector(), new Vector()]); + + faces[0][0] = a; + faces[0][1] = b; + faces[0][2] = c; + faces[0][3] = b.sub(a).cross(c.sub(a)).normalized(); // ABC + faces[1][0] = a; + faces[1][1] = c; + faces[1][2] = d; + faces[1][3] = c.sub(a).cross(d.sub(a)).normalized(); + faces[2][0] = a; + faces[2][1] = d; + faces[2][2] = b; + faces[2][3] = d.sub(a).cross(b.sub(a)).normalized(); + faces[3][0] = b; + faces[3][1] = d; + faces[3][2] = c; + faces[3][3] = d.sub(b).cross(c.sub(b)).normalized(); + + var numFaces = 4; + var closestFace = 0; + + for (iteration in 0...maxEpaIterations) { + // Find face that's closest to origin + var min_dist = faces[0][0].dot(faces[0][3]); + closestFace = 0; + for (i in 1...numFaces) { + var dist = faces[i][0].dot(faces[i][3]); + if (dist < min_dist) { + min_dist = dist; + closestFace = i; + } + } + + // search normal to face that's closest to origin + var search_dir = faces[closestFace][3]; + var p = hull.support(search_dir).sub(sphere.support(search_dir.multiply(-1))); + if (p.dot(search_dir) - min_dist < epaTolerance) { + // Convergence (new point is not significantly further from origin) + return faces[closestFace][3].multiply(p.dot(search_dir)); // dot vertex with normal to resolve collision along normal! + } + var loose_edges = []; + for (i in 0...maxEpaLooseEdges) + loose_edges.push([new Vector(), new Vector()]); + + var num_loose_edges = 0; + + // Find all triangles that are facing p + var i = 0; + while (i < numFaces) { + if (faces[i][3].dot(p.sub(faces[i][0])) > 0) // triangle i faces p, remove it + { + // Add removed triangle's edges to loose edge list. + // If it's already there, remove it (both triangles it belonged to are gone) + for (j in 0...3) // Three edges per face + { + var current_edge = [faces[i][j], faces[i][(j + 1) % 3]]; + var found_edge = false; + for (k in 0...num_loose_edges) // Check if current edge is already in list + { + if (loose_edges[k][1] == current_edge[0] && loose_edges[k][0] == current_edge[1]) { + // Edge is already in the list, remove it + // THIS ASSUMES EDGE CAN ONLY BE SHARED BY 2 TRIANGLES (which should be true) + // THIS ALSO ASSUMES SHARED EDGE WILL BE REVERSED IN THE TRIANGLES (which + // should be true provided every triangle is wound CCW) + loose_edges[k][0] = loose_edges[num_loose_edges - 1][0]; // Overwrite current edge + loose_edges[k][1] = loose_edges[num_loose_edges - 1][1]; // with last edge in list + num_loose_edges--; + found_edge = true; + break; + // exit loop because edge can only be shared once + } + } // endfor loose_edges + + if (!found_edge) { // add current edge to list + // assert(num_loose_edges= maxEpaLooseEdges) + break; + loose_edges[num_loose_edges][0] = current_edge[0]; + loose_edges[num_loose_edges][1] = current_edge[1]; + num_loose_edges++; + } + } + + // Remove triangle i from list + faces[i][0] = faces[numFaces - 1][0]; + faces[i][1] = faces[numFaces - 1][1]; + faces[i][2] = faces[numFaces - 1][2]; + faces[i][3] = faces[numFaces - 1][3]; + numFaces--; + i--; + } // endif p can see triangle i + + i++; + } // endfor num_faces + + // Reconstruct polytope with p added + for (i in 0...num_loose_edges) { + // assert(num_faces= maxEpaFaces) + break; + faces[numFaces][0] = loose_edges[i][0]; + faces[numFaces][1] = loose_edges[i][1]; + faces[numFaces][2] = p; + faces[numFaces][3] = loose_edges[i][0].sub(loose_edges[i][1]).cross(loose_edges[i][0].sub(p)).normalized(); + + // Check for wrong normal to maintain CCW winding + var bias = 0.000001; // in case dot result is only slightly < 0 (because origin is on face) + if (faces[numFaces][0].dot(faces[numFaces][3]) + bias < 0) { + var temp = faces[numFaces][0]; + faces[numFaces][0] = faces[numFaces][1]; + faces[numFaces][1] = temp; + faces[numFaces][3] = faces[numFaces][3].multiply(-1); + } + numFaces++; + } + } + return faces[closestFace][3].multiply(faces[closestFace][0].dot(faces[closestFace][3])); + } +} diff --git a/src/collision/gjk/Sphere.hx b/src/collision/gjk/Sphere.hx new file mode 100644 index 00000000..2f7074be --- /dev/null +++ b/src/collision/gjk/Sphere.hx @@ -0,0 +1,19 @@ +package collision.gjk; + +import h3d.Vector; + +@:publicFields +class Sphere { + var position:Vector; + var radius:Float; + + public function new() {} + + public function support(direction:Vector) { + var c = position.clone(); + var d = direction.normalized(); + d = d.multiply(radius); + c = c.add(d); + return c; + } +}