diff --git a/src/oimo/collision/geometry/GeometryType.hx b/src/oimo/collision/geometry/GeometryType.hx index 953749c..8c08c40 100644 --- a/src/oimo/collision/geometry/GeometryType.hx +++ b/src/oimo/collision/geometry/GeometryType.hx @@ -11,6 +11,7 @@ class GeometryType { public static inline var _CONE:Int = 3; public static inline var _CAPSULE:Int = 4; public static inline var _CONVEX_HULL:Int = 5; + public static inline var _STATIC_MESH:Int = 6; public static inline var _CONVEX_MIN:Int = 0; public static inline var _CONVEX_MAX:Int = 5; @@ -56,4 +57,11 @@ class GeometryType { * See `ConvexHullGeometry`. */ public static var CONVEX_HULL(default, never):Int = _CONVEX_HULL; + + /** + * Represents a static mesh collision geometry. + * + * See `StaticMeshGeometry`. + */ + public static var STATIC_MESH(default, never):Int = _STATIC_MESH; } diff --git a/src/oimo/collision/geometry/StaticMeshGeometry.hx b/src/oimo/collision/geometry/StaticMeshGeometry.hx new file mode 100644 index 0000000..3b74067 --- /dev/null +++ b/src/oimo/collision/geometry/StaticMeshGeometry.hx @@ -0,0 +1,239 @@ +package oimo.collision.geometry; +import haxe.ds.Vector; +import oimo.collision.broadphase.bvh.BvhNode; +import oimo.collision.broadphase.bvh.BvhProxy; +import oimo.collision.broadphase.bvh.BvhTree; +import oimo.common.MathUtil; +import oimo.common.Transform; +import oimo.common.Vec3; +import oimo.m.IVec3; +import oimo.m.M; + +/** + * Static mesh collision geometry with BVH spatial queries. + */ +@:build(oimo.m.B.bu()) +class StaticMeshGeometry extends Geometry { + public var _vertices:Vector; + public var _indices:Vector; + public var _normals:Vector; + public var _numVertices:Int; + public var _numTriangles:Int; + var _triangleAABBs:Vector; + var _triangleBVH:BvhTree; + var _triangleProxies:Vector; + var _nextProxyId:Int; + + public function new(vertices:Array, indices:Array, computeNormals:Bool = true) { + super(GeometryType._STATIC_MESH); + _numVertices = vertices.length; + _numTriangles = Std.int(indices.length / 3); + + _vertices = new Vector(_numVertices); + for (i in 0..._numVertices) _vertices[i] = new Vec3().copyFrom(vertices[i]); + + _indices = new Vector(indices.length); + for (i in 0...indices.length) _indices[i] = indices[i]; + + _normals = new Vector(_numTriangles); + if (computeNormals) _computeTriangleNormals(); + else for (i in 0..._numTriangles) _normals[i] = new Vec3(0,1,0); + + _buildTriangleAABBs(); + _buildTriangleBVH(); + _updateMass(); + } + + public inline function getVertices():Vector return _vertices; + public inline function getIndices():Vector return _indices; + public inline function getNumTriangles():Int return _numTriangles; + + public function getTriangleVertices(triangleIndex:Int, v1:Vec3, v2:Vec3, v3:Vec3):Void { + var idx = triangleIndex * 3; + var i1 = _indices[idx]; + var i2 = _indices[idx + 1]; + var i3 = _indices[idx + 2]; + v1.copyFrom(_vertices[i1]); + v2.copyFrom(_vertices[i2]); + v3.copyFrom(_vertices[i3]); + } + + public function getTriangleNormal(triangleIndex:Int, out:Vec3):Void { + out.copyFrom(_normals[triangleIndex]); + } + + public function queryTriangles(aabb:Aabb):Array { + var results:Array = []; + _queryBVHRecursive(_triangleBVH._root, aabb, results); + return results; + } + + function _queryBVHRecursive(node:BvhNode, queryAABB:Aabb, results:Array):Void { + if (node == null) return; + + var nodeAABB = new Aabb(); + M.vec3_assign(nodeAABB._min, node._aabbMin); + M.vec3_assign(nodeAABB._max, node._aabbMax); + + if (!nodeAABB.overlap(queryAABB)) return; + + if (node._height == 0) { + var triangleIndex:Int = cast node._proxy.userData; + results.push(triangleIndex); + } else { + _queryBVHRecursive(node._children[0], queryAABB, results); + _queryBVHRecursive(node._children[1], queryAABB, results); + } + } + + override public function _rayCastLocal(begin:IVec3, end:IVec3, hit:RayCastHit):Bool { + var rayDir:IVec3; + M.vec3_sub(rayDir, end, begin); + var rayLengthSq:Float = M.vec3_dot(rayDir, rayDir); + if (rayLengthSq < 1e-12) return false; + + var rayLength:Float = MathUtil.sqrt(rayLengthSq); + M.vec3_scale(rayDir, rayDir, 1.0 / rayLength); + + var rayAABB = new Aabb(); + var beginX = M.vec3_get(begin, 0); var beginY = M.vec3_get(begin, 1); var beginZ = M.vec3_get(begin, 2); + var endX = M.vec3_get(end, 0); var endY = M.vec3_get(end, 1); var endZ = M.vec3_get(end, 2); + M.vec3_set(rayAABB._min, + beginX < endX ? beginX : endX, + beginY < endY ? beginY : endY, + beginZ < endZ ? beginZ : endZ + ); + M.vec3_set(rayAABB._max, + beginX > endX ? beginX : endX, + beginY > endY ? beginY : endY, + beginZ > endZ ? beginZ : endZ + ); + + var triangles:Array = queryTriangles(rayAABB); + var closestT:Float = MathUtil.POSITIVE_INFINITY; + var closestTriangle:Int = -1; + + for (i in 0...triangles.length) { + var triangleIdx = triangles[i]; + var idx:Int = triangleIdx * 3; + var i1:Int = _indices[idx]; + var i2:Int = _indices[idx + 1]; + var i3:Int = _indices[idx + 2]; + + var v1:IVec3; M.vec3_fromVec3(v1, _vertices[i1]); + var v2:IVec3; M.vec3_fromVec3(v2, _vertices[i2]); + var v3:IVec3; M.vec3_fromVec3(v3, _vertices[i3]); + + var edge1:IVec3; M.vec3_sub(edge1, v2, v1); + var edge2:IVec3; M.vec3_sub(edge2, v3, v1); + var h:IVec3; M.vec3_cross(h, rayDir, edge2); + var a:Float = M.vec3_dot(edge1, h); + + if (a > -1e-6 && a < 1e-6) continue; + + var f:Float = 1.0 / a; + var s:IVec3; M.vec3_sub(s, begin, v1); + var u:Float = f * M.vec3_dot(s, h); + + if (u < 0.0 || u > 1.0) continue; + + var q:IVec3; M.vec3_cross(q, s, edge1); + var v:Float = f * M.vec3_dot(rayDir, q); + + if (v < 0.0 || u + v > 1.0) continue; + + var t:Float = f * M.vec3_dot(edge2, q); + + if (t > 1e-6 && t <= rayLength && t < closestT) { + closestT = t; + closestTriangle = triangleIdx; + } + } + + if (closestTriangle >= 0) { + var hitPos:IVec3; + M.vec3_addRhsScaled(hitPos, begin, rayDir, closestT); + + var hitNormal:IVec3; + M.vec3_fromVec3(hitNormal, _normals[closestTriangle]); + + M.vec3_toVec3(hit.position, hitPos); + M.vec3_toVec3(hit.normal, hitNormal); + hit.fraction = closestT / rayLength; + return true; + } + + return false; + } + + override public function _updateMass():Void { + var minx = MathUtil.POSITIVE_INFINITY; var miny = MathUtil.POSITIVE_INFINITY; var minz = MathUtil.POSITIVE_INFINITY; + var maxx = MathUtil.NEGATIVE_INFINITY; var maxy = MathUtil.NEGATIVE_INFINITY; var maxz = MathUtil.NEGATIVE_INFINITY; + for (i in 0..._numVertices) { + var v = _vertices[i]; + if (v.x < minx) minx = v.x; if (v.x > maxx) maxx = v.x; + if (v.y < miny) miny = v.y; if (v.y > maxy) maxy = v.y; + if (v.z < minz) minz = v.z; if (v.z > maxz) maxz = v.z; + } + var sizex = maxx - minx; var sizey = maxy - miny; var sizez = maxz - minz; + _volume = sizex * sizey * sizez; + M.mat3_diagonal(_inertiaCoeff, + 1 / 12 * (sizey * sizey + sizez * sizez), + 1 / 12 * (sizez * sizez + sizex * sizex), + 1 / 12 * (sizex * sizex + sizey * sizey) + ); + } + + override public function _computeAabb(aabb:Aabb, tf:Transform):Void { + var minx = MathUtil.POSITIVE_INFINITY; var miny = MathUtil.POSITIVE_INFINITY; var minz = MathUtil.POSITIVE_INFINITY; + var maxx = MathUtil.NEGATIVE_INFINITY; var maxy = MathUtil.NEGATIVE_INFINITY; var maxz = MathUtil.NEGATIVE_INFINITY; + for (i in 0..._numVertices) { + var localV:IVec3; M.vec3_fromVec3(localV, _vertices[i]); + var worldV:IVec3; M.vec3_mulMat3(worldV, localV, tf._rotation); M.vec3_add(worldV, worldV, tf._position); + var x = M.vec3_get(worldV, 0); var y = M.vec3_get(worldV, 1); var z = M.vec3_get(worldV, 2); + 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; + } + M.vec3_set(aabb._min, minx, miny, minz); + M.vec3_set(aabb._max, maxx, maxy, maxz); + } + + function _computeTriangleNormals():Void { + for (i in 0..._numTriangles) { + var idx = i * 3; + var i1 = _indices[idx]; var i2 = _indices[idx + 1]; var i3 = _indices[idx + 2]; + var v1 = _vertices[i1]; var v2 = _vertices[i2]; var v3 = _vertices[i3]; + var edge1 = new Vec3().copyFrom(v2).subEq(v1); + var edge2 = new Vec3().copyFrom(v3).subEq(v1); + var normal = new Vec3().copyFrom(edge1).crossEq(edge2).normalize(); + _normals[i] = normal; + } + } + + function _buildTriangleAABBs():Void { + _triangleAABBs = new Vector(_numTriangles); + for (i in 0..._numTriangles) { + var v1 = new Vec3(); var v2 = new Vec3(); var v3 = new Vec3(); + getTriangleVertices(i, v1, v2, v3); + var aabb = new Aabb(); + var minx = Math.min(v1.x, Math.min(v2.x, v3.x)); var miny = Math.min(v1.y, Math.min(v2.y, v3.y)); var minz = Math.min(v1.z, Math.min(v2.z, v3.z)); + var maxx = Math.max(v1.x, Math.max(v2.x, v3.x)); var maxy = Math.max(v1.y, Math.max(v2.y, v3.y)); var maxz = Math.max(v1.z, Math.max(v2.z, v3.z)); + M.vec3_set(aabb._min, minx, miny, minz); M.vec3_set(aabb._max, maxx, maxy, maxz); + _triangleAABBs[i] = aabb; + } + } + + function _buildTriangleBVH():Void { + _triangleBVH = new BvhTree(); + _triangleProxies = new Vector(_numTriangles); + _nextProxyId = 0; + + for (i in 0..._numTriangles) { + var proxy = new BvhProxy(i, _nextProxyId++); + proxy._setAabb(_triangleAABBs[i]); + _triangleBVH._insertProxy(proxy); + _triangleProxies[i] = proxy; + } + } +} diff --git a/src/oimo/collision/narrowphase/CollisionMatrix.hx b/src/oimo/collision/narrowphase/CollisionMatrix.hx index 31d253e..d9ba35f 100644 --- a/src/oimo/collision/narrowphase/CollisionMatrix.hx +++ b/src/oimo/collision/narrowphase/CollisionMatrix.hx @@ -6,6 +6,7 @@ import oimo.collision.narrowphase.detector.GjkEpaDetector; import oimo.collision.narrowphase.detector.SphereBoxDetector; import oimo.collision.narrowphase.detector.SphereCapsuleDetector; import oimo.collision.narrowphase.detector.SphereSphereDetector; +import oimo.collision.narrowphase.detector.ConvexStaticMeshDetector; import oimo.collision.geometry.GeometryType; import oimo.collision.narrowphase.detector.*; @@ -19,7 +20,7 @@ class CollisionMatrix { @:dox(hide) public function new() { detectors = new Vector>(8); - for (i in 0...6) { + for (i in 0...7) { detectors[i] = new Vector(8); } @@ -31,6 +32,7 @@ class CollisionMatrix { var co:Int = GeometryType._CONE; var ca:Int = GeometryType._CAPSULE; var ch:Int = GeometryType._CONVEX_HULL; + var sm:Int = GeometryType._STATIC_MESH; detectors[sp][sp] = new SphereSphereDetector(); detectors[sp][bo] = new SphereBoxDetector(false); @@ -73,6 +75,23 @@ class CollisionMatrix { detectors[ch][co] = gjkEpaDetector; detectors[ch][ca] = gjkEpaDetector; detectors[ch][ch] = gjkEpaDetector; + + // Static mesh detectors - single detector handles all convex shapes + var convexStaticMeshDetector = new ConvexStaticMeshDetector(false); + detectors[sp][sm] = convexStaticMeshDetector; + detectors[bo][sm] = convexStaticMeshDetector; + detectors[cy][sm] = convexStaticMeshDetector; + detectors[co][sm] = convexStaticMeshDetector; + detectors[ca][sm] = convexStaticMeshDetector; + detectors[ch][sm] = convexStaticMeshDetector; + + var convexStaticMeshDetectorSwapped = new ConvexStaticMeshDetector(true); + detectors[sm][sp] = convexStaticMeshDetectorSwapped; + detectors[sm][bo] = convexStaticMeshDetectorSwapped; + detectors[sm][cy] = convexStaticMeshDetectorSwapped; + detectors[sm][co] = convexStaticMeshDetectorSwapped; + detectors[sm][ca] = convexStaticMeshDetectorSwapped; + detectors[sm][ch] = convexStaticMeshDetectorSwapped; } // --- public --- diff --git a/src/oimo/collision/narrowphase/detector/ConvexStaticMeshDetector.hx b/src/oimo/collision/narrowphase/detector/ConvexStaticMeshDetector.hx new file mode 100644 index 0000000..0e98450 --- /dev/null +++ b/src/oimo/collision/narrowphase/detector/ConvexStaticMeshDetector.hx @@ -0,0 +1,146 @@ +package oimo.collision.narrowphase.detector; +import oimo.collision.geometry.*; +import oimo.collision.narrowphase.*; +import oimo.collision.narrowphase.detector.gjkepa.*; +import oimo.common.Transform; +import oimo.common.Vec3; +import oimo.m.IVec3; +import oimo.m.M; + +/** + * ConvexGeometry vs StaticMeshGeometry collision detector. + */ +@:build(oimo.m.B.bu()) +class ConvexStaticMeshDetector extends Detector { + + public function new(swapped:Bool = false) { + super(swapped); + } + + override function detectImpl(result:DetectorResult, geom1:Geometry, geom2:Geometry, tf1:Transform, tf2:Transform, cachedData:CachedDetectorData):Void { + var convex:ConvexGeometry = cast geom1; + var staticMesh:StaticMeshGeometry = cast geom2; + + result.incremental = false; + + var worldAABB:Aabb = new Aabb(); + convex._computeAabb(worldAABB, tf1); + + var localAABB:Aabb = new Aabb(); + _transformAABBToLocal(worldAABB, tf2, localAABB); + + var triangleIndices:Array = staticMesh.queryTriangles(localAABB); + if (triangleIndices.length == 0) { + return; + } + + var gjkEpa:GjkEpa = GjkEpa.getInstance(); + var bestPenetrationDepth:Float = -Math.POSITIVE_INFINITY; + var bestContactFound:Bool = false; + var bestNormalWorld:Vec3 = new Vec3(); + var bestContactPointConvex:Vec3 = new Vec3(); + var bestContactPointMesh:Vec3 = new Vec3(); + + // Reuse triangle geometry to avoid allocations + var triangleVertices:Array = [new Vec3(), new Vec3(), new Vec3()]; + var triangleGeom:ConvexHullGeometry = null; + for (triangleIndex in triangleIndices) { + staticMesh.getTriangleVertices(triangleIndex, triangleVertices[0], triangleVertices[1], triangleVertices[2]); + + var v1 = triangleVertices[0]; + var v2 = triangleVertices[1]; + var v3 = triangleVertices[2]; + + // Skip degenerate triangles + var edge1 = new Vec3().copyFrom(v2).subEq(v1); + var edge2 = new Vec3().copyFrom(v3).subEq(v1); + var cross = edge1.cross(edge2); + if (cross.lengthSq() < 1e-12) { + continue; + } + + if (triangleGeom == null) { + triangleGeom = new ConvexHullGeometry(triangleVertices); + } else { + triangleGeom._vertices[0].copyFrom(v1); + triangleGeom._vertices[1].copyFrom(v2); + triangleGeom._vertices[2].copyFrom(v3); + triangleGeom._updateMass(); + } + triangleGeom._gjkMargin = 0; + + var status:Int = gjkEpa.computeClosestPoints(convex, triangleGeom, tf1, tf2, null); + + if (status != GjkEpaResultState.SUCCEEDED) { + continue; + } + + var margin1:Float = convex._gjkMargin; + var margin2:Float = triangleGeom._gjkMargin; + + if (gjkEpa.distance > margin1 + margin2) { + continue; + } + + var penetrationDepth:Float = margin1 + margin2 - gjkEpa.distance; + + if (penetrationDepth > bestPenetrationDepth) { + bestPenetrationDepth = penetrationDepth; + bestContactFound = true; + + bestNormalWorld.copyFrom(gjkEpa.closestPoint1).subEq(gjkEpa.closestPoint2); + if (gjkEpa.distance < 0) { + bestNormalWorld.negateEq(); + } + bestNormalWorld.normalize(); + + bestContactPointConvex.copyFrom(gjkEpa.closestPoint1).addScaledEq(bestNormalWorld, -margin1); + bestContactPointMesh.copyFrom(gjkEpa.closestPoint2).addScaledEq(bestNormalWorld, margin2); + } + } + + if (bestContactFound) { + var normalInWorld:IVec3; + M.vec3_fromVec3(normalInWorld, bestNormalWorld); + + setNormal(result, M.vec3_get(normalInWorld, 0), M.vec3_get(normalInWorld, 1), M.vec3_get(normalInWorld, 2)); + + var contactPointConvex:IVec3; + var contactPointMesh:IVec3; + M.vec3_fromVec3(contactPointConvex, bestContactPointConvex); + M.vec3_fromVec3(contactPointMesh, bestContactPointMesh); + + addPoint(result, + M.vec3_get(contactPointConvex, 0), M.vec3_get(contactPointConvex, 1), M.vec3_get(contactPointConvex, 2), + M.vec3_get(contactPointMesh, 0), M.vec3_get(contactPointMesh, 1), M.vec3_get(contactPointMesh, 2), + bestPenetrationDepth, 0); + } + } + + // Helper method to transform AABB from world space to mesh local space + function _transformAABBToLocal(worldAABB:Aabb, meshTransform:Transform, localAABB:Aabb):Void { + var minCorner:IVec3; + var maxCorner:IVec3; + var transformedCorner:IVec3; + + M.vec3_assign(minCorner, worldAABB._min); + M.vec3_sub(minCorner, minCorner, meshTransform._position); + M.vec3_mulMat3Transposed(minCorner, minCorner, meshTransform._rotation); + M.vec3_assign(localAABB._min, minCorner); + M.vec3_assign(localAABB._max, minCorner); + + for (i in 0...8) { + M.vec3_set(transformedCorner, + (i & 1) != 0 ? M.vec3_get(worldAABB._max, 0) : M.vec3_get(worldAABB._min, 0), + (i & 2) != 0 ? M.vec3_get(worldAABB._max, 1) : M.vec3_get(worldAABB._min, 1), + (i & 4) != 0 ? M.vec3_get(worldAABB._max, 2) : M.vec3_get(worldAABB._min, 2) + ); + + M.vec3_sub(transformedCorner, transformedCorner, meshTransform._position); + M.vec3_mulMat3Transposed(transformedCorner, transformedCorner, meshTransform._rotation); + + M.vec3_min(localAABB._min, localAABB._min, transformedCorner); + M.vec3_max(localAABB._max, localAABB._max, transformedCorner); + } + } +} \ No newline at end of file diff --git a/src/oimo/dynamics/World.hx b/src/oimo/dynamics/World.hx index 99c882e..2b525c3 100644 --- a/src/oimo/dynamics/World.hx +++ b/src/oimo/dynamics/World.hx @@ -356,6 +356,8 @@ class World { _drawCapsule(d, cast geom, tf, color); case GeometryType._CONVEX_HULL: _drawConvexHull(d, cast geom, tf, color); + case GeometryType._STATIC_MESH: + _drawStaticMesh(d, cast geom, tf, color); } } @@ -443,6 +445,38 @@ class World { _pool.dispose(o); } + extern inline function _drawStaticMesh(d:DebugDraw, g:StaticMeshGeometry, tf:Transform, color:Vec3):Void { + var m:Mat3 = _pool.mat3(); + var o:Vec3 = _pool.vec3(); + tf.getRotationTo(m); + tf.getPositionTo(o); + var v1:Vec3 = _pool.vec3(); + var v2:Vec3 = _pool.vec3(); + var v3:Vec3 = _pool.vec3(); + var n:Vec3 = _pool.vec3(); + for (i in 0...g.getNumTriangles()) { + g.getTriangleVertices(i, v1, v2, v3); + g.getTriangleNormal(i, n); + v1.mulMat3Eq(m).addEq(o); + v2.mulMat3Eq(m).addEq(o); + v3.mulMat3Eq(m).addEq(o); + n.mulMat3Eq(m); + if (d.wireframe) { + d.line(v1, v2, color); + d.line(v2, v3, color); + d.line(v3, v1, color); + } else { + d.triangle(v1, v2, v3, n, n, n, color); + } + } + _pool.dispose(v1); + _pool.dispose(v2); + _pool.dispose(v3); + _pool.dispose(n); + _pool.dispose(m); + _pool.dispose(o); + } + extern inline function _drawAabb(d:DebugDraw, aabb:Aabb, color:Vec3):Void { var min:Vec3 = _pool.vec3(); var max:Vec3 = _pool.vec3(); @@ -665,12 +699,12 @@ class World { var rylm:RotationalLimitMotor = j._rotLms[1]; var rzlm:RotationalLimitMotor = j._rotLms[2]; _drawTranslationalLimit3D(d, anchor1, basisX1, basisY1, basisZ1, txlm, tylm, tzlm, color); - + var rotYAxis:Vec3 = _pool.vec3(); M.vec3_toVec3(rotYAxis, j._axisY); var rotYBasisX:Vec3 = _pool.vec3().copyFrom(basisX1); var rotYBasisY:Vec3 = _pool.vec3().copyFrom(basisX1).crossEq(rotYAxis); - + _drawRotationalLimit(d, anchor2, basisY1, basisZ1, basisY1, radius, j._angleX - rxlm.upperLimit, j._angleX - rxlm.lowerLimit, color); _drawRotationalLimit(d, anchor2, rotYBasisX, rotYBasisY, rotYBasisX, radius, rylm.lowerLimit - j._angleY, rylm.upperLimit - j._angleY, color); _drawRotationalLimit(d, anchor2, basisX2, basisY2, basisX2, radius, rzlm.lowerLimit - j._angleZ, rzlm.upperLimit - j._angleZ, color);