diff --git a/src/Marble.hx b/src/Marble.hx index c93dd0f3..c2fd1010 100644 --- a/src/Marble.hx +++ b/src/Marble.hx @@ -729,6 +729,301 @@ class Marble extends GameObject { rPitch.value = pitch; } + function testMove(velocity:Vector, position:Vector, deltaT:Float, radius:Float, testPIs:Bool):{position:Vector, t:Float} { + var velLen = velocity.length(); + if (velLen < 0.001) + return {position: position, t: deltaT}; + + var velocityDir = velocity.normalized(); + + var deltaPosition = velocity.multiply(deltaT); + var finalPosition = position.add(deltaPosition); + + var searchbox = new Bounds(); + searchbox.addSpherePos(this.x, this.y, this.z, _radius); + searchbox.addSpherePos(this.x + velocity.x * deltaT * 2, this.y + velocity.y * deltaT * 2, this.z + velocity.z * deltaT * 2, _radius); + + var foundObjs = this.level.collisionWorld.boundingSearch(searchbox); + + var finalT = deltaT; + var marbleCollisionTime = finalT; + var marbleCollisionNormal = new Vector(0, 0, 1); + + var lastContactPos = new Vector(); + + function toDifPoint(vec:Vector) { + return new Point3F(vec.x, vec.y, vec.z); + } + function fromDifPoint(vec:Point3F) { + return new Vector(vec.x, vec.y, vec.z); + } + + var contactPoly:{v0:Vector, v:Vector, v2:Vector}; + + for (obj in foundObjs) { + // Its an MP so bruh + + var invMatrix = @:privateAccess obj.invTransform; + if (obj.go is PathedInterior) + invMatrix = obj.transform.getInverse(); + var localpos = position.clone(); + localpos.transform(invMatrix); + + var relLocalVel = velocity.sub(obj.velocity); + relLocalVel.transform3x3(invMatrix); + + var boundThing = new Bounds(); + boundThing.addSpherePos(localpos.x, localpos.y, localpos.z, radius * 1.1); + boundThing.addSpherePos(localpos.x + + relLocalVel.x * deltaT * 2, localpos.y + + relLocalVel.y * deltaT * 2, localpos.z + + relLocalVel.z * deltaT * 2, + radius * 1.1); + + var surfaces = obj.octree.boundingSearch(boundThing); + + for (surf in surfaces) { + var surface:CollisionSurface = cast surf; + + var i = 0; + while (i < surface.indices.length) { + var v0 = surface.points[surface.indices[i]].transformed(obj.transform); + var v = surface.points[surface.indices[i + 1]].transformed(obj.transform); + var v2 = surface.points[surface.indices[i + 2]].transformed(obj.transform); + + var polyPlane = PlaneF.ThreePoints(toDifPoint(v0), toDifPoint(v), toDifPoint(v2)); + + // If we're going the wrong direction or not going to touch the plane, ignore... + if (!(polyPlane.getNormal().dot(toDifPoint(velocityDir)) > -0.001 + || polyPlane.getNormal().dot(toDifPoint(finalPosition)) + polyPlane.d > radius)) { + // Time until collision with the plane + var collisionTime = (radius + - (polyPlane.getNormal().dot(toDifPoint(position)) + polyPlane.d)) / polyPlane.getNormal().dot(toDifPoint(velocity)); + + // Are we going to touch the plane during this time step? + if (collisionTime >= 0.0 && finalT >= collisionTime) { + var lastVertIndex = surface.indices[surface.indices.length - 1]; + var lastVert = surface.points[lastVertIndex]; + + var collisionPos = velocity.multiply(collisionTime).add(position); + + var isOnEdge:Bool = false; + + for (i in 0...surface.indices.length) { + { + var thisVert = surface.points[surface.indices[i]]; + if (thisVert != lastVert) { + var edgePlane = PlaneF.ThreePoints(toDifPoint(thisVert).add(polyPlane.getNormal()), toDifPoint(thisVert), + toDifPoint(lastVert)); + lastVert = thisVert; + + // if we are on the far side of the edge + if (edgePlane.getNormal().dot(toDifPoint(collisionPos)) + edgePlane.d < 0.0) + break; + } + } + + isOnEdge = i != surface.indices.length; + } + + // If we're inside the poly, just get the position + if (!isOnEdge) { + finalT = collisionTime; + finalPosition = collisionPos; + lastContactPos = fromDifPoint(polyPlane.project(toDifPoint(collisionPos))); + contactPoly = {v0: v0, v: v, v2: v2}; + i += 3; + continue; + } + } + + // We *might* be colliding with an edge + + var lastVert = surface.points[surface.indices[surface.indices.length - 1]]; + + if (surface.indices.length == 0) { + i += 3; + continue; + } + var radSq = radius * radius; + for (iter in 0...surface.indices.length) { + var thisVert = surface.points[surface.indices[i]]; + + var vertDiff = lastVert.sub(thisVert); + var posDiff = position.sub(thisVert); + + var velRejection = vertDiff.cross(velocity); + var posRejection = vertDiff.cross(posDiff); + + // Build a quadratic equation to solve for the collision time + var a = velRejection.lengthSq(); + var halfB = posRejection.dot(velRejection); + var b = halfB + halfB; + + var discriminant = b * b - (posRejection.lengthSq() - vertDiff.lengthSq() * radSq) * (a * 4.0); + + // If it's not quadratic or has no solution, ignore this edge. + if (a == 0.0 || discriminant < 0.0) { + lastVert = thisVert; + continue; + } + + var oneOverTwoA = 0.5 / a; + var discriminantSqrt = Math.sqrt(discriminant); + + // Solve using the quadratic formula + var edgeCollisionTime = (discriminantSqrt - b) * oneOverTwoA; + var edgeCollisionTime2 = (-b - discriminantSqrt) * oneOverTwoA; + + // Make sure the 2 times are in ascending order + if (edgeCollisionTime2 < edgeCollisionTime) { + var temp = edgeCollisionTime2; + edgeCollisionTime2 = edgeCollisionTime; + edgeCollisionTime = temp; + } + + // If the collision doesn't happen on this time step, ignore this edge. + if (edgeCollisionTime2 <= 0.0001 || finalT <= edgeCollisionTime) { + lastVert = thisVert; + continue; + } + + // Check if the collision hasn't already happened + if (edgeCollisionTime >= 0.0) { + var edgeLen = vertDiff.length(); + + var relativeCollisionPos = velocity.multiply(edgeCollisionTime).add(position).sub(thisVert); + + var distanceAlongEdge = relativeCollisionPos.dot(vertDiff) / edgeLen; + + // If the collision happens outside the boundaries of the edge, ignore this edge. + if (-radius > distanceAlongEdge || edgeLen + radius < distanceAlongEdge) { + lastVert = thisVert; + continue; + } + + // If the collision is within the edge, resolve the collision and continue. + if (distanceAlongEdge >= 0.0 && distanceAlongEdge <= edgeLen) { + finalT = edgeCollisionTime; + finalPosition = velocity.multiply(edgeCollisionTime).add(position); + + lastContactPos = vertDiff.multiply(distanceAlongEdge / edgeLen).add(thisVert); + contactPoly = {v0: v0, v: v, v2: v2}; + + lastVert = thisVert; + continue; + } + } + + // This is what happens when we collide with a corner + + var speedSq = velocity.lengthSq(); + + // Build a quadratic equation to solve for the collision time + var posVertDiff = position.sub(thisVert); + var halfCornerB = posVertDiff.dot(velocity); + var cornerB = halfCornerB + halfCornerB; + + var fourA = speedSq * 4.0; + + var cornerDiscriminant = cornerB * cornerB - (posVertDiff.lengthSq() - radSq) * fourA; + + // If it's quadratic and has a solution ... + if (speedSq != 0.0 && cornerDiscriminant >= 0.0) { + var oneOver2A = 0.5 / speedSq; + var cornerDiscriminantSqrt = Math.sqrt(cornerDiscriminant); + + // Solve using the quadratic formula + var cornerCollisionTime = (cornerDiscriminantSqrt - cornerB) * oneOver2A; + var cornerCollisionTime2 = (-cornerB - cornerDiscriminantSqrt) * oneOver2A; + + // Make sure the 2 times are in ascending order + if (cornerCollisionTime2 < cornerCollisionTime) { + var temp = cornerCollisionTime2; + cornerCollisionTime2 = cornerCollisionTime; + cornerCollisionTime = temp; + } + + // If the collision doesn't happen on this time step, ignore this corner + if (cornerCollisionTime2 > 0.0001 && finalT > cornerCollisionTime) { + // Adjust to make sure very small negative times are counted as zero + if (cornerCollisionTime <= 0.0 && cornerCollisionTime > -0.0001) + cornerCollisionTime = 0.0; + + // Check if the collision hasn't already happened + if (cornerCollisionTime >= 0.0) { + // Resolve it and continue + finalT = cornerCollisionTime; + contactPoly = {v0: v0, v: v, v2: v2}; + finalPosition = velocity.multiply(cornerCollisionTime).add(position); + lastContactPos = thisVert; + } + } + } + + // We still need to check the other corner ... + // Build one last quadratic equation to solve for the collision time + var lastVertDiff = position.sub(lastVert); + var lastCornerHalfB = lastVertDiff.dot(velocity); + var lastCornerB = lastCornerHalfB + lastCornerHalfB; + var lastCornerDiscriminant = lastCornerB * lastCornerB - (lastVertDiff.lengthSq() - radSq) * fourA; + + // If it's not quadratic or has no solution, then skip this corner + if (speedSq == 0.0 || lastCornerDiscriminant < 0.0) { + lastVert = thisVert; + continue; + } + + var lastCornerOneOver2A = 0.5 / speedSq; + var lastCornerDiscriminantSqrt = Math.sqrt(lastCornerDiscriminant); + + // Solve using the quadratic formula + var lastCornerCollisionTime = (lastCornerDiscriminantSqrt - lastCornerB) * lastCornerOneOver2A; + var lastCornerCollisionTime2 = (-lastCornerB - lastCornerDiscriminantSqrt) * lastCornerOneOver2A; + + // Make sure the 2 times are in ascending order + if (lastCornerCollisionTime2 < lastCornerCollisionTime) { + var temp = lastCornerCollisionTime2; + lastCornerCollisionTime2 = lastCornerCollisionTime; + lastCornerCollisionTime = temp; + } + + // If the collision doesn't happen on this time step, ignore this corner + if (lastCornerCollisionTime2 <= 0.0001 || finalT <= lastCornerCollisionTime) { + lastVert = thisVert; + continue; + } + + // Adjust to make sure very small negative times are counted as zero + if (lastCornerCollisionTime <= 0.0 && lastCornerCollisionTime > -0.0001) + lastCornerCollisionTime = 0.0; + + // Check if the collision hasn't already happened + if (lastCornerCollisionTime < 0.0) { + lastVert = thisVert; + continue; + } + + // Resolve it and continue + finalT = lastCornerCollisionTime; + finalPosition = velocity.multiply(lastCornerCollisionTime).add(position); + lastContactPos = lastVert; + contactPoly = {v0: v0, v: v, v2: v2}; + + lastVert = thisVert; + } + } + + i += 3; + } + } + } + + position = finalPosition; + + return {position: position, t: finalT}; + } + function getIntersectionTime(dt:Float, velocity:Vector) { var searchbox = new Bounds(); searchbox.addSpherePos(this.x, this.y, this.z, _radius); @@ -757,6 +1052,8 @@ class Marble extends GameObject { var radius = _radius; var invMatrix = @:privateAccess obj.invTransform; + if (obj.go is PathedInterior) + invMatrix = obj.transform.getInverse(); var localpos = position.clone(); localpos.transform(invMatrix); @@ -795,12 +1092,13 @@ class Marble extends GameObject { foundTriangles.push(v2); // foundTriangles.push(surfacenormal); - traceinfo.resetTrace(position.clone(), position.add(velocity.multiply(dt)), this._radius); + traceinfo.resetTrace(position.clone(), position.add(relVelocity.multiply(dt)), this._radius); traceinfo.traceSphereTriangle(v2, v, v0); if (traceinfo.collision) { var tcolpos = traceinfo.getTraceEndpoint(); - var closest = Collision.ClosestPtPointTriangle(tcolpos, _radius, v2, v, v0, surfacenormal); + var closest = Collision.ClosestPtPointTriangle(tcolpos, _radius, v2.add(obj.velocity.multiply(traceinfo.t)), + v.add(obj.velocity.multiply(traceinfo.t)), v0.add(obj.velocity.multiply(traceinfo.t)), surfacenormal); if (closest != null) { var dist = tcolpos.sub(closest); var distlen = dist.length(); @@ -846,10 +1144,26 @@ class Marble extends GameObject { var pt = GJK.gjk(sph, chull); - if (pt != null) { + while (pt != null) { + if (pt.lengthSq() < 0.0001) { + break; + } + trace('Separating Vector Len: ${pt.length()}'); finalPos = finalPos.sub(pt); + sph.position = finalPos; + pt = GJK.gjk(sph, chull); } + // if (pt != null) { + // finalPos = finalPos.sub(pt); + // sph.position = finalPos; + // pt = GJK.gjk(sph, chull); + // if (pt != null) { + // trace("?????"); + // } + // trace('Separating Vector Len: ${pt.length()}'); + // } + // var colpos = finalPos; // var msh = new h3d.prim.Sphere(); // var prim = new h3d.scene.Mesh(msh); @@ -887,7 +1201,7 @@ class Marble extends GameObject { if (timeRemaining <= 0) break; - var timeStep = 0.002; + var timeStep = 0.004; if (timeRemaining < timeStep) timeStep = timeRemaining; @@ -898,16 +1212,18 @@ class Marble extends GameObject { } } - var intersectT = this.getIntersectionTime(timeStep, velocity); + var intersectData = testMove(velocity, this.getAbsPos().getPosition(), timeStep, _radius, true); // this.getIntersectionTime(timeStep, velocity); + var intersectT = intersectData.t; - if (intersectT < timeStep && intersectT >= 0.00001) { + if (intersectT < timeStep && intersectT >= 0.0001) { // trace('CCD AT t = ${intersectT}'); - intersectT *= 0.8; // We uh tick the shit to not actually at the contact time cause bruh + // intersectT *= 0.8; // We uh tick the shit to not actually at the contact time cause bruh // intersectT /= 2; var diff = timeStep - intersectT; // this.velocity = this.velocity.sub(A.multiply(diff)); // this.omega = this.omega.sub(a.multiply(diff)); timeStep = intersectT; + // this.setPosition(intersectData.position.x, intersectData.position.y, intersectData.position.z); } var tempState = timeState.clone();