mirror of
https://github.com/RandomityGuy/MBHaxe.git
synced 2026-02-18 12:11:03 +00:00
replace CCD function with what MBG used
This commit is contained in:
RandomityGuy
parent
255658f456
commit
201aa1f82d
1 changed files with 323 additions and 7 deletions
330
src/Marble.hx
330
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();
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue