mirror of
https://github.com/RandomityGuy/MBHaxe.git
synced 2025-10-30 08:11:25 +00:00
cleanup and fix marble-marble collision
This commit is contained in:
RandomityGuy
parent
536e86b8bb
commit
2d4065ccd8
6 changed files with 36 additions and 686 deletions
|
|
@ -86,14 +86,14 @@ class Main extends hxd.App {
|
|||
|
||||
// s3d.camera.
|
||||
|
||||
// var marble2 = new Marble();
|
||||
// world.addMarble(marble2);
|
||||
// marble2.setPosition(5, 0, 5);
|
||||
|
||||
var marble = new Marble();
|
||||
marble.controllable = true;
|
||||
world.addMarble(marble);
|
||||
marble.setPosition(0, 0, 2);
|
||||
|
||||
var marble2 = new Marble();
|
||||
world.addMarble(marble2);
|
||||
marble2.setPosition(5, 0, 5);
|
||||
// marble.setPosition(-10, -5, 5);
|
||||
}
|
||||
|
||||
|
|
|
|||
370
src/Marble.hx
370
src/Marble.hx
|
|
@ -192,6 +192,8 @@ class Marble extends Object {
|
|||
var normP = contacts[i].normal.multiply(dp.dot(contacts[i].normal));
|
||||
normP = normP.multiply(bounce + 1);
|
||||
|
||||
this.velocity = this.velocity.sub(normP.multiply(1 / ourMass));
|
||||
|
||||
otherMarble.velocity = otherMarble.velocity.add(normP.multiply(1 / theirMass));
|
||||
contacts[i].velocity = otherMarble.velocity;
|
||||
} else {
|
||||
|
|
@ -296,25 +298,25 @@ class Marble extends Object {
|
|||
var bestContact = (bestSurface != -1) ? contacts[bestSurface] : new CollisionInfo();
|
||||
var canJump = bestSurface != -1;
|
||||
if (canJump && m.jump) {
|
||||
var velDifference = this.velocity.clone().sub(bestContact.velocity);
|
||||
var velDifference = this.velocity.sub(bestContact.velocity);
|
||||
var sv = bestContact.normal.dot(velDifference);
|
||||
if (sv < 0) {
|
||||
sv = 0;
|
||||
}
|
||||
if (sv < this._jumpImpulse) {
|
||||
this.velocity = this.velocity.add(bestContact.normal.clone().multiply((this._jumpImpulse - sv)));
|
||||
this.velocity = this.velocity.add(bestContact.normal.multiply((this._jumpImpulse - sv)));
|
||||
}
|
||||
}
|
||||
for (j in 0...contacts.length) {
|
||||
var normalForce2 = -contacts[j].normal.dot(A);
|
||||
if (normalForce2 > 0 && contacts[j].normal.dot(this.velocity.clone().sub(contacts[j].velocity)) <= 0.0001) {
|
||||
if (normalForce2 > 0 && contacts[j].normal.dot(this.velocity.sub(contacts[j].velocity)) <= 0.0001) {
|
||||
A = A.add(contacts[j].normal.multiply(normalForce2));
|
||||
}
|
||||
}
|
||||
if (bestSurface != -1) {
|
||||
// TODO: FIX
|
||||
// bestContact.velocity - bestContact.normal * Vector3.Dot(bestContact.normal, bestContact.velocity);
|
||||
var vAtC = this.velocity.clone().add(this.omega.clone().cross(bestContact.normal.clone().multiply(-this._radius))).sub(bestContact.velocity);
|
||||
var vAtC = this.velocity.add(this.omega.cross(bestContact.normal.multiply(-this._radius))).sub(bestContact.velocity);
|
||||
var vAtCMag = vAtC.length();
|
||||
var slipping = false;
|
||||
var aFriction = new Vector(0, 0, 0);
|
||||
|
|
@ -331,27 +333,24 @@ class Marble extends Object {
|
|||
AMagnitude *= fraction;
|
||||
slipping = false;
|
||||
}
|
||||
var vAtCDir = vAtC.clone().multiply(1 / vAtCMag);
|
||||
aFriction = bestContact.normal.clone()
|
||||
.multiply(-1)
|
||||
.cross(vAtCDir.clone().multiply(-1))
|
||||
.multiply(angAMagnitude);
|
||||
AFriction = vAtCDir.clone().multiply(-AMagnitude);
|
||||
var vAtCDir = vAtC.multiply(1 / vAtCMag);
|
||||
aFriction = bestContact.normal.cross(vAtCDir).multiply(angAMagnitude);
|
||||
AFriction = vAtCDir.multiply(-AMagnitude);
|
||||
this._slipAmount = vAtCMag - totalDeltaV;
|
||||
}
|
||||
if (!slipping) {
|
||||
var R = gWorkGravityDir.clone().multiply(-this._radius);
|
||||
var R = gWorkGravityDir.multiply(-this._radius);
|
||||
var aadd = R.cross(A).multiply(1 / R.lengthSq());
|
||||
if (isCentered) {
|
||||
var nextOmega = this.omega.add(a.clone().multiply(dt));
|
||||
var nextOmega = this.omega.add(a.multiply(dt));
|
||||
aControl = desiredOmega.clone().sub(nextOmega);
|
||||
var aScalar = aControl.length();
|
||||
if (aScalar > this._brakingAcceleration) {
|
||||
aControl = aControl.multiply(this._brakingAcceleration / aScalar);
|
||||
}
|
||||
}
|
||||
var Aadd = aControl.clone().cross(bestContact.normal.multiply(-this._radius)).multiply(-1);
|
||||
var aAtCMag = aadd.clone().cross(bestContact.normal.multiply(-this._radius)).add(Aadd).length();
|
||||
var Aadd = aControl.cross(bestContact.normal.multiply(this._radius));
|
||||
var aAtCMag = aadd.cross(bestContact.normal.multiply(-this._radius)).add(Aadd).length();
|
||||
var friction2 = this._staticFriction * bestContact.friction;
|
||||
|
||||
if (aAtCMag > friction2 * bestNormalForce) {
|
||||
|
|
@ -378,294 +377,6 @@ class Marble extends Object {
|
|||
this._bounceNormal = normal;
|
||||
}
|
||||
|
||||
function testMove(velocity:Vector, position:Vector, deltaT:Float, radius:Float, testPIs:Bool, collisionWorld:CollisionWorld) {
|
||||
var velLen = velocity.length();
|
||||
if (velLen < 0.001)
|
||||
return deltaT;
|
||||
|
||||
var velocityDir = velocity.normalized();
|
||||
|
||||
var deltaPosition = velocity.multiply(deltaT);
|
||||
var finalPosition = position.add(deltaPosition);
|
||||
|
||||
var expandedcollider = new SphereCollisionEntity(cast this);
|
||||
expandedcollider.transform = Matrix.T(position.x, position.y, position.z);
|
||||
expandedcollider.radius = this.getAbsPos().getPosition().distance(position) + radius;
|
||||
|
||||
var foundObjs = collisionWorld.radiusSearch(position, expandedcollider.radius);
|
||||
|
||||
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) {
|
||||
if (obj.velocity.length() != 0) { // Its an MP so bruh
|
||||
|
||||
var invMatrix = obj.transform.clone();
|
||||
invMatrix.invert();
|
||||
var localpos = position.clone();
|
||||
localpos.transform(invMatrix);
|
||||
var surfaces = obj.octree.radiusSearch(localpos, expandedcollider.radius);
|
||||
|
||||
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;
|
||||
|
||||
var contacted = false;
|
||||
if (deltaT > finalT) {
|
||||
contacted = true;
|
||||
}
|
||||
|
||||
return finalT;
|
||||
}
|
||||
|
||||
function getIntersectionTime(dt:Float, velocity:Vector, pathedInteriors:Array<PathedInterior>, collisionWorld:CollisionWorld) {
|
||||
var expandedcollider = new SphereCollisionEntity(cast this);
|
||||
var position = this.getAbsPos().getPosition();
|
||||
|
|
@ -814,61 +525,6 @@ class Marble extends Object {
|
|||
|
||||
advancePhysics(currentTime, dt, move, collisionWorld, pathedInteriors);
|
||||
|
||||
// var timeRemaining = dt;
|
||||
// var it = 0;
|
||||
|
||||
// var piTime = currentTime;
|
||||
|
||||
// var pos = this.getAbsPos().getPosition();
|
||||
// do {
|
||||
// if (timeRemaining <= 0)
|
||||
// break;
|
||||
|
||||
// var timeStep = 0.00800000037997961;
|
||||
// if (timeRemaining < 0.00800000037997961)
|
||||
// timeStep = timeRemaining;
|
||||
|
||||
// var externalForces = getExternalForces(move, timeStep);
|
||||
|
||||
// advancePhysics(move, timeStep, collisionWorld);
|
||||
|
||||
// var intersectT = this.getIntersectionTime(timeStep, velocity, pathedInteriors, collisionWorld);
|
||||
|
||||
// if (intersectT < timeStep) {
|
||||
// timeStep = intersectT;
|
||||
// }
|
||||
|
||||
// piTime += timeStep;
|
||||
|
||||
// if (this.controllable) {
|
||||
// for (interior in pathedInteriors) {
|
||||
// // interior.rollBack();
|
||||
// interior.update(piTime, timeStep);
|
||||
// }
|
||||
// }
|
||||
|
||||
// var pos = this.getAbsPos().getPosition();
|
||||
|
||||
// var newPos = pos.add(this.velocity.multiply(timeStep));
|
||||
// this.setPosition(newPos.x, newPos.y, newPos.z);
|
||||
// var tform = this.collider.transform;
|
||||
// tform.setPosition(new Vector(newPos.x, newPos.y, newPos.z));
|
||||
// this.collider.setTransform(tform);
|
||||
// this.collider.velocity = this.velocity;
|
||||
|
||||
// // if (intersectT != timeStep && intersectT != 10e8) {
|
||||
// // // this.velocity = this.velocity.sub(externalForces.multiply(timeStep - intersectT));
|
||||
// // // this.omega
|
||||
// // if (intersectT > timeStep) {
|
||||
// // trace("Bruh");
|
||||
// // }
|
||||
// // timeStep = intersectT;
|
||||
// // }
|
||||
|
||||
// timeRemaining -= timeStep;
|
||||
// it++;
|
||||
// } while (it <= 10);
|
||||
|
||||
this.camera.target.load(this.getAbsPos().getPosition().toPoint());
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -324,248 +324,4 @@ class Collision {
|
|||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
public static function CheckTriangle(packet:CollisionPacket, p1:Vector, p2:Vector, p3:Vector) {
|
||||
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 plane = PlaneF.ThreePoints(toDifPoint(p1), toDifPoint(p2), toDifPoint(p3));
|
||||
|
||||
// only check front facing triangles
|
||||
var dist = plane.distance(toDifPoint(packet.e_norm_velocity));
|
||||
if (dist < 0) {
|
||||
return packet;
|
||||
}
|
||||
|
||||
// get interval of plane intersection
|
||||
var t0:Float = 0.0;
|
||||
var t1:Float = 0.0;
|
||||
var embedded_in_plane:Bool = false;
|
||||
|
||||
// signed distance from sphere to point on plane
|
||||
var signed_dist_to_plane:Float = plane.distance(toDifPoint(packet.e_base_point));
|
||||
|
||||
// cache this as we will reuse
|
||||
var normal_dot_vel = plane.getNormal().dot(toDifPoint(packet.e_velocity));
|
||||
|
||||
// if sphere is moving parrallel to plane
|
||||
if (normal_dot_vel == 0.0) {
|
||||
if (Math.abs(signed_dist_to_plane) >= 1.0) {
|
||||
// no collision possible
|
||||
return packet;
|
||||
} else {
|
||||
// sphere is in plane in whole range [0..1]
|
||||
embedded_in_plane = true;
|
||||
t0 = 0.0;
|
||||
t1 = 1.0;
|
||||
}
|
||||
} else {
|
||||
// N dot D is not 0, calc intersect interval
|
||||
t0 = (-1.0 - signed_dist_to_plane) / normal_dot_vel;
|
||||
t1 = (1.0 - signed_dist_to_plane) / normal_dot_vel;
|
||||
|
||||
// swap so t0 < t1
|
||||
if (t0 > t1) {
|
||||
var temp = t1;
|
||||
t1 = t0;
|
||||
t0 = temp;
|
||||
}
|
||||
|
||||
// check that at least one result is within range
|
||||
if (t0 > 1.0 || t1 < 0.0) {
|
||||
// both values outside range [0,1] so no collision
|
||||
return packet;
|
||||
}
|
||||
|
||||
// clamp to [0,1]
|
||||
if (t0 < 0.0) {
|
||||
t0 = 0.0;
|
||||
}
|
||||
if (t1 < 0.0) {
|
||||
t1 = 0.0;
|
||||
}
|
||||
if (t0 > 1.0) {
|
||||
t0 = 1.0;
|
||||
}
|
||||
if (t1 > 1.0) {
|
||||
t1 = 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
// time to check for a collision
|
||||
var collision_point:Vector = new Vector(0.0, 0.0, 0.0);
|
||||
var found_collision:Bool = false;
|
||||
var t:Float = 1.0;
|
||||
|
||||
// first check collision with the inside of the triangle
|
||||
if (!embedded_in_plane) {
|
||||
var plane_intersect:Vector = packet.e_base_point.sub(fromDifPoint(plane.getNormal()));
|
||||
var temp:Vector = packet.e_velocity.multiply(t0);
|
||||
plane_intersect = plane_intersect.add(temp);
|
||||
|
||||
if (Collision.PointInTriangle(plane_intersect, p1, p2, p3)) {
|
||||
found_collision = true;
|
||||
t = t0;
|
||||
collision_point = plane_intersect;
|
||||
}
|
||||
}
|
||||
|
||||
// no collision yet, check against points and edges
|
||||
if (!found_collision) {
|
||||
var velocity = packet.e_velocity.clone();
|
||||
var base = packet.e_base_point.clone();
|
||||
|
||||
var velocity_sq_length = velocity.lengthSq();
|
||||
var a:Float = velocity_sq_length;
|
||||
var b:Float = 0.0;
|
||||
var c:Float = 0.0;
|
||||
|
||||
// equation is a*t^2 + b*t + c = 0
|
||||
// check against points
|
||||
|
||||
// p1
|
||||
var temp = base.sub(p1);
|
||||
b = 2.0 * velocity.dot(temp);
|
||||
temp = p1.sub(base);
|
||||
c = temp.lengthSq() - 1.0;
|
||||
var new_t = Collision.GetLowestRoot(a, b, c, t);
|
||||
if (new_t != null) {
|
||||
t = new_t;
|
||||
found_collision = true;
|
||||
collision_point = p1;
|
||||
}
|
||||
|
||||
// p2
|
||||
if (!found_collision) {
|
||||
temp = base.sub(p2);
|
||||
b = 2.0 * velocity.dot(temp);
|
||||
temp = p2.sub(base);
|
||||
c = temp.lengthSq() - 1.0;
|
||||
new_t = Collision.GetLowestRoot(a, b, c, t);
|
||||
if (new_t != null) {
|
||||
t = new_t;
|
||||
found_collision = true;
|
||||
collision_point = p2;
|
||||
}
|
||||
}
|
||||
|
||||
// p3
|
||||
if (!found_collision) {
|
||||
temp = base.sub(p3);
|
||||
b = 2.0 * velocity.dot(temp);
|
||||
temp = p3.sub(base);
|
||||
c = temp.lengthSq() - 1.0;
|
||||
new_t = Collision.GetLowestRoot(a, b, c, t);
|
||||
if (new_t != null) {
|
||||
t = new_t;
|
||||
found_collision = true;
|
||||
collision_point = p3;
|
||||
}
|
||||
}
|
||||
|
||||
// check against edges
|
||||
// p1 -> p2
|
||||
var edge = p2.sub(p1);
|
||||
var base_to_vertex = p1.sub(base);
|
||||
var edge_sq_length = edge.lengthSq();
|
||||
var edge_dot_velocity = edge.dot(velocity);
|
||||
var edge_dot_base_to_vertex = edge.dot(base_to_vertex);
|
||||
|
||||
// calculate params for equation
|
||||
a = edge_sq_length * -velocity_sq_length + edge_dot_velocity * edge_dot_velocity;
|
||||
b = edge_sq_length * (2.0 * velocity.dot(base_to_vertex)) - 2.0 * edge_dot_velocity * edge_dot_base_to_vertex;
|
||||
c = edge_sq_length * (1.0 - base_to_vertex.lengthSq()) + edge_dot_base_to_vertex * edge_dot_base_to_vertex;
|
||||
|
||||
// do we collide against infinite edge
|
||||
new_t = Collision.GetLowestRoot(a, b, c, t);
|
||||
if (new_t != null) {
|
||||
// check if intersect is within line segment
|
||||
var f = (edge_dot_velocity * new_t - edge_dot_base_to_vertex) / edge_sq_length;
|
||||
if (f >= 0.0 && f <= 1.0) {
|
||||
t = new_t;
|
||||
found_collision = true;
|
||||
collision_point = p1.add(edge.multiply(f));
|
||||
}
|
||||
}
|
||||
|
||||
// p2 -> p3
|
||||
edge = p3.sub(p2);
|
||||
base_to_vertex = p2.sub(base);
|
||||
edge_sq_length = edge.lengthSq();
|
||||
edge_dot_velocity = edge.dot(velocity);
|
||||
edge_dot_base_to_vertex = edge.dot(base_to_vertex);
|
||||
|
||||
// calculate params for equation
|
||||
a = edge_sq_length * -velocity_sq_length + edge_dot_velocity * edge_dot_velocity;
|
||||
b = edge_sq_length * (2.0 * velocity.dot(base_to_vertex)) - 2.0 * edge_dot_velocity * edge_dot_base_to_vertex;
|
||||
c = edge_sq_length * (1.0 - base_to_vertex.lengthSq()) + edge_dot_base_to_vertex * edge_dot_base_to_vertex;
|
||||
|
||||
// do we collide against infinite edge
|
||||
new_t = Collision.GetLowestRoot(a, b, c, t);
|
||||
if (new_t != null) {
|
||||
// check if intersect is within line segment
|
||||
var f = (edge_dot_velocity * new_t - edge_dot_base_to_vertex) / edge_sq_length;
|
||||
if (f >= 0.0 && f <= 1.0) {
|
||||
t = new_t;
|
||||
found_collision = true;
|
||||
collision_point = p2.add(edge.multiply(f));
|
||||
}
|
||||
}
|
||||
|
||||
// p3 -> p1
|
||||
edge = p1.sub(p3);
|
||||
base_to_vertex = p3.sub(base);
|
||||
edge_sq_length = edge.lengthSq();
|
||||
edge_dot_velocity = edge.dot(velocity);
|
||||
edge_dot_base_to_vertex = edge.dot(base_to_vertex);
|
||||
|
||||
// calculate params for equation
|
||||
a = edge_sq_length * -velocity_sq_length + edge_dot_velocity * edge_dot_velocity;
|
||||
b = edge_sq_length * (2.0 * velocity.dot(base_to_vertex)) - 2.0 * edge_dot_velocity * edge_dot_base_to_vertex;
|
||||
c = edge_sq_length * (1.0 - base_to_vertex.lengthSq()) + edge_dot_base_to_vertex * edge_dot_base_to_vertex;
|
||||
|
||||
// do we collide against infinite edge
|
||||
new_t = Collision.GetLowestRoot(a, b, c, t);
|
||||
if (new_t != null) {
|
||||
// check if intersect is within line segment
|
||||
var f = (edge_dot_velocity * new_t - edge_dot_base_to_vertex) / edge_sq_length;
|
||||
if (f >= 0.0 && f <= 1.0) {
|
||||
t = new_t;
|
||||
found_collision = true;
|
||||
collision_point = p3.add(edge.multiply(f));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// set results
|
||||
if (found_collision) {
|
||||
// distance to collision, t is time of collision
|
||||
var dist_to_coll = t * packet.e_velocity.length();
|
||||
|
||||
// are we the closest hit?
|
||||
if (!packet.found_collision || dist_to_coll < packet.nearest_distance) {
|
||||
packet.nearest_distance = dist_to_coll;
|
||||
packet.intersect_point = collision_point;
|
||||
packet.found_collision = true;
|
||||
}
|
||||
|
||||
// HACK: USE SENSORS FOR THIS AND YOU DON'T GET WALL HITS ANYMORE
|
||||
// Work out the hit normal so we can determine if the player is in
|
||||
// contact with a wall or the ground.
|
||||
var n = collision_point.sub(packet.e_base_point);
|
||||
n.normalize();
|
||||
|
||||
var dz = n.dot(new Vector(0, 0, 1));
|
||||
if (dz <= -0.5) {
|
||||
packet.grounded = true;
|
||||
}
|
||||
}
|
||||
|
||||
return packet;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@ class CollisionInfo {
|
|||
public var restitution:Float;
|
||||
public var contactDistance:Float;
|
||||
public var force:Float;
|
||||
public var penetration:Float;
|
||||
|
||||
public function new() {}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,67 +0,0 @@
|
|||
package collision;
|
||||
|
||||
import h3d.Vector;
|
||||
|
||||
@:publicFields
|
||||
class CollisionPacket {
|
||||
static var LARGE_NUMBER:Float = 1e20;
|
||||
|
||||
/** Position in world space. Assumes Z-up. **/
|
||||
var r3_position = new Vector(0.0, 0.0, 0.0);
|
||||
|
||||
/** Velocity in world space. Assumes Z-up. **/
|
||||
var r3_velocity = new Vector(0.0, 0.0, 0.0);
|
||||
|
||||
/** Ellipsoid radius in world units. Assumes Z-up. **/
|
||||
var e_radius = new Vector(1.0, 1.0, 1.0);
|
||||
|
||||
/** Position in ellipsoid-space. Used internally. **/
|
||||
var e_position = new Vector(0.0, 0.0, 0.0);
|
||||
|
||||
/** Velocity in ellipsoid-space. Used internally. **/
|
||||
var e_velocity = new Vector(0.0, 0.0, 0.0);
|
||||
|
||||
/** Found a collision. **/
|
||||
var found_collision = false;
|
||||
|
||||
/** Distance to nearest collision if `found_collision` is set, otherwise `1e20` **/
|
||||
var nearest_distance = LARGE_NUMBER;
|
||||
|
||||
/** Iteration depth. Useful for debugging slow collisions. **/
|
||||
var depth:Int = 0;
|
||||
|
||||
// internal stuff
|
||||
var e_norm_velocity = new Vector(0.0, 0.0, 0.0);
|
||||
var e_base_point = new Vector(0.0, 0.0, 0.0);
|
||||
var intersect_point = new Vector(0.0, 0.0, 0.0);
|
||||
|
||||
/** `true` if packet is on something reasonable to call the ground.
|
||||
*
|
||||
* _in practice, you probably want a sensor in your game code instead._ **/
|
||||
var grounded:Bool = false;
|
||||
|
||||
/** Recalculate e-space from r3 vectors. Call this if you updated `r3_*` **/
|
||||
inline function to_e() {
|
||||
this.e_position = new Vector(this.r3_position.x / this.e_radius.x, this.r3_position.y / this.e_radius.y, this.r3_position.z / this.e_radius.z);
|
||||
this.e_velocity = new Vector(this.r3_velocity.x / this.e_radius.x, this.r3_velocity.y / this.e_radius.y, this.r3_velocity.z / this.e_radius.z);
|
||||
}
|
||||
|
||||
/** Recalculate e-space from r3 vectors. Call this if you updated `e_*` **/
|
||||
inline function to_r3() {
|
||||
this.r3_position = new Vector(this.e_position.x * this.e_radius.x, this.e_position.y * this.e_radius.y, this.e_position.z * this.e_radius.z);
|
||||
this.r3_velocity = new Vector(this.e_velocity.x * this.e_radius.x, this.e_velocity.y * this.e_radius.y, this.e_velocity.z * this.e_radius.z);
|
||||
}
|
||||
|
||||
inline function new(?position:Vector, ?velocity:Vector, ?radius:Vector) {
|
||||
if (position != null) {
|
||||
this.r3_position = position;
|
||||
}
|
||||
if (velocity != null) {
|
||||
this.r3_velocity = velocity;
|
||||
}
|
||||
if (radius != null) {
|
||||
this.e_radius = radius;
|
||||
}
|
||||
this.to_e();
|
||||
}
|
||||
}
|
||||
|
|
@ -37,30 +37,34 @@ class SphereCollisionEntity extends CollisionEntity {
|
|||
var velocity = collisionEntity.velocity;
|
||||
var radius = collisionEntity.radius;
|
||||
|
||||
if (position.distanceSq(thispos) < (radius + this.radius) * (radius + this.radius)) {
|
||||
var otherDist = thispos.sub(position);
|
||||
var otherRadius = this.radius + radius;
|
||||
|
||||
if (otherRadius * otherRadius * 1.01 > otherDist.lengthSq()) {
|
||||
var normDist = otherDist.normalized();
|
||||
var contact = new CollisionInfo();
|
||||
contact.collider = this;
|
||||
contact.friction = 1;
|
||||
contact.restitution = 1;
|
||||
contact.velocity = this.velocity;
|
||||
contact.point = position.add(thispos).multiply(0.5);
|
||||
contact.normal = contact.point.sub(thispos).normalized();
|
||||
contact.point = position.add(normDist);
|
||||
contact.normal = normDist.multiply(-1);
|
||||
contact.force = 0;
|
||||
contact.contactDistance = contact.point.distance(position);
|
||||
// contact.penetration = radius - (position.sub(contact.point).dot(contact.normal));
|
||||
contact.penetration = radius - (position.sub(contact.point).dot(contact.normal));
|
||||
contacts.push(contact);
|
||||
|
||||
var othercontact = new CollisionInfo();
|
||||
othercontact.collider = collisionEntity;
|
||||
othercontact.friction = 1;
|
||||
othercontact.restitution = 1;
|
||||
othercontact.velocity = this.velocity;
|
||||
othercontact.point = thispos.add(position).multiply(0.5);
|
||||
othercontact.normal = contact.point.sub(position).normalized();
|
||||
othercontact.contactDistance = contact.point.distance(position);
|
||||
othercontact.force = 0;
|
||||
// var othercontact = new CollisionInfo();
|
||||
// othercontact.collider = collisionEntity;
|
||||
// othercontact.friction = 1;
|
||||
// othercontact.restitution = 1;
|
||||
// othercontact.velocity = this.velocity;
|
||||
// othercontact.point = thispos.add(position).multiply(0.5);
|
||||
// othercontact.normal = contact.point.sub(position).normalized();
|
||||
// othercontact.contactDistance = contact.point.distance(position);
|
||||
// othercontact.force = 0;
|
||||
// othercontact.penetration = this.radius - (thispos.sub(othercontact.point).dot(othercontact.normal));
|
||||
this.marble.queueCollision(othercontact);
|
||||
// this.marble.queueCollision(othercontact);
|
||||
}
|
||||
return contacts;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue