mirror of
https://github.com/RandomityGuy/MBHaxe.git
synced 2025-10-30 08:11:25 +00:00
276 lines
6.9 KiB
Haxe
276 lines
6.9 KiB
Haxe
package collision;
|
||
|
||
import h3d.Vector;
|
||
|
||
@:publicFields
|
||
class TraceInfo {
|
||
var start:Vector;
|
||
var end:Vector;
|
||
var scaledStart:Vector;
|
||
var radius:Float;
|
||
var invRadius:Float;
|
||
var vel:Vector;
|
||
var scaledVel:Vector;
|
||
var velLength:Float;
|
||
var normVel:Vector;
|
||
var collision:Bool;
|
||
var t:Float;
|
||
var intersectPoint:Vector;
|
||
var tmp:Vector;
|
||
|
||
public function new() {
|
||
this.start = new Vector();
|
||
this.end = new Vector();
|
||
this.scaledStart = new Vector();
|
||
this.radius = 0;
|
||
this.invRadius = 0;
|
||
this.vel = new Vector();
|
||
this.scaledVel = new Vector();
|
||
this.velLength = 0;
|
||
this.normVel = new Vector();
|
||
this.collision = false;
|
||
this.t = 0;
|
||
this.intersectPoint = new Vector();
|
||
}
|
||
|
||
public function resetTrace(start:Vector, end:Vector, radius:Float) {
|
||
this.invRadius = 1 / radius;
|
||
this.radius = radius;
|
||
|
||
this.start.set(start.x, start.y, start.z);
|
||
this.end.set(end.x, end.y, end.z);
|
||
this.vel = this.end.sub(this.start);
|
||
this.normVel = this.vel.normalized();
|
||
|
||
this.scaledStart = start.multiply(this.invRadius);
|
||
this.scaledVel = this.vel.multiply(this.invRadius);
|
||
|
||
this.velLength = this.vel.length();
|
||
|
||
this.collision = false;
|
||
this.t = 1.0;
|
||
}
|
||
|
||
public function setCollision(t:Float, point:Vector) {
|
||
this.collision = true;
|
||
if (t < this.t) {
|
||
this.t = t;
|
||
this.intersectPoint = point.multiply(this.radius);
|
||
}
|
||
}
|
||
|
||
public function getTraceEndpoint() {
|
||
return this.start.add(this.vel.multiply(this.t));
|
||
}
|
||
|
||
public function getTraceDistance() {
|
||
return this.vel.multiply(this.t).length();
|
||
}
|
||
|
||
public function traceSphereTriangle(a:Vector, b:Vector, c:Vector) {
|
||
var invRadius = this.invRadius;
|
||
var vel = this.scaledVel;
|
||
var start = this.scaledStart;
|
||
|
||
// Scale the triangle points so that we're colliding against a unit-radius sphere.
|
||
var ta = a.multiply(invRadius);
|
||
var tb = b.multiply(invRadius);
|
||
var tc = c.multiply(invRadius);
|
||
|
||
// Calculate triangle normal.
|
||
// This may be better to do as a pre-process
|
||
var pab = tb.sub(ta);
|
||
var pac = tc.sub(ta);
|
||
var norm = pab.cross(pac);
|
||
norm.normalize();
|
||
var planeD = -(norm.dot(ta));
|
||
|
||
// Colliding against the backface of the triangle
|
||
if (norm.dot(this.normVel) >= 0) {
|
||
// Two choices at this point:
|
||
|
||
// 1) Negate the normal so that it always points towards the start point
|
||
// This feels kludgy, but I'm not sure if there's a better alternative
|
||
/*vec3.negate(norm, norm);
|
||
planeD = -planeD; */
|
||
|
||
// 2) Or allow it to pass through
|
||
return;
|
||
}
|
||
|
||
// Get interval of plane intersection:
|
||
var t0, t1;
|
||
var embedded = false;
|
||
|
||
// Calculate the signed distance from sphere
|
||
// position to triangle plane
|
||
var distToPlane = start.dot(norm) + planeD;
|
||
|
||
// cache this as we’re going to use it a few times below:
|
||
var normDotVel = norm.dot(vel);
|
||
|
||
if (normDotVel == 0.0) {
|
||
// Sphere is travelling parrallel to the plane:
|
||
if (Math.abs(distToPlane) >= 1.0) {
|
||
// Sphere is not embedded in plane, No collision possible
|
||
return;
|
||
} else {
|
||
// Sphere is completely embedded in plane.
|
||
// It intersects in the whole range [0..1]
|
||
embedded = true;
|
||
t0 = 0.0;
|
||
t1 = 1.0;
|
||
}
|
||
} else {
|
||
// Calculate intersection interval:
|
||
t0 = (-1.0 - distToPlane) / normDotVel;
|
||
t1 = (1.0 - distToPlane) / normDotVel;
|
||
// 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) {
|
||
// No collision possible
|
||
return;
|
||
}
|
||
// 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;
|
||
}
|
||
|
||
// If the closest possible collision point is further away
|
||
// than an already detected collision then there's no point
|
||
// in testing further.
|
||
if (t0 >= this.t) {
|
||
return;
|
||
}
|
||
|
||
// t0 and t1 now represent the range of the sphere movement
|
||
// during which it intersects with the triangle plane.
|
||
// Collisions cannot happen outside that range.
|
||
|
||
// Check for collision againt the triangle face:
|
||
if (!embedded) {
|
||
// Calculate the intersection point with the plane
|
||
var planeIntersect = start.sub(norm);
|
||
var v = vel.multiply(t0);
|
||
planeIntersect = planeIntersect.add(v);
|
||
|
||
// Is that point inside the triangle?
|
||
if (pointInTriangle(planeIntersect, ta, tb, tc)) {
|
||
this.setCollision(t0, planeIntersect);
|
||
// Collisions against the face will always be closer than vertex or edge collisions
|
||
// so we can stop checking now.
|
||
return;
|
||
}
|
||
}
|
||
|
||
var velSqrLen = vel.lengthSq();
|
||
var t = this.t;
|
||
|
||
// Check for collision againt the triangle vertices:
|
||
t = testVertex(ta, velSqrLen, t, start, vel);
|
||
t = testVertex(tb, velSqrLen, t, start, vel);
|
||
t = testVertex(tc, velSqrLen, t, start, vel);
|
||
|
||
// Check for collision against the triangle edges:
|
||
t = testEdge(ta, tb, velSqrLen, t, start, vel);
|
||
t = testEdge(tb, tc, velSqrLen, t, start, vel);
|
||
testEdge(tc, ta, velSqrLen, t, start, vel);
|
||
}
|
||
|
||
function pointInTriangle(p:Vector, t0:Vector, t1:Vector, t2:Vector) {
|
||
var pt0 = t0.sub(p);
|
||
var pt1 = t1.sub(p);
|
||
var pt2 = t2.sub(p);
|
||
|
||
pt0.normalize();
|
||
pt1.normalize();
|
||
pt2.normalize();
|
||
|
||
var a = pt0.dot(pt1);
|
||
var b = pt1.dot(pt2);
|
||
var c = pt2.dot(pt0);
|
||
|
||
var angle = Math.acos(a) + Math.acos(b) + Math.acos(c);
|
||
|
||
// If the point is on the triangle all the interior angles should add up to 360 deg.
|
||
var collision = Math.abs(angle - (2 * Math.PI)) < 0.01;
|
||
return collision;
|
||
}
|
||
|
||
function getLowestRoot(a:Float, b:Float, c:Float, maxR:Float) {
|
||
var det = b * b - 4.0 * a * c;
|
||
if (det < 0) {
|
||
return -1.0;
|
||
}
|
||
|
||
var sqrtDet = Math.sqrt(det);
|
||
var r1 = (-b - sqrtDet) / (2.0 * a);
|
||
var r2 = (-b + sqrtDet) / (2.0 * a);
|
||
|
||
if (r1 > r2) {
|
||
var tmp = r2;
|
||
r2 = r1;
|
||
r1 = tmp;
|
||
}
|
||
|
||
if (r1 > 0 && r1 < maxR) {
|
||
return r1;
|
||
}
|
||
|
||
if (r2 > 0 && r2 < maxR) {
|
||
return r2;
|
||
}
|
||
|
||
return -1.0;
|
||
}
|
||
|
||
function testVertex(p:Vector, velSqrLen:Float, t:Float, start:Vector, vel:Vector) {
|
||
var v = start.sub(p);
|
||
var b = 2.0 * vel.dot(v);
|
||
var c = v.lengthSq() - 1.0;
|
||
var newT = getLowestRoot(velSqrLen, b, c, t);
|
||
if (newT != -1) {
|
||
this.setCollision(newT, p);
|
||
return newT;
|
||
}
|
||
return t;
|
||
}
|
||
|
||
function testEdge(pa:Vector, pb:Vector, velSqrLen:Float, t:Float, start:Vector, vel:Vector) {
|
||
var edge = pb.sub(pa);
|
||
var v = pa.sub(start);
|
||
|
||
var edgeSqrLen = edge.lengthSq();
|
||
var edgeDotVel = edge.dot(vel);
|
||
var edgeDotSphereVert = edge.dot(v);
|
||
|
||
var a = edgeSqrLen * -velSqrLen + edgeDotVel * edgeDotVel;
|
||
var b = edgeSqrLen * (2.0 * vel.dot(v)) - 2.0 * edgeDotVel * edgeDotSphereVert;
|
||
var c = edgeSqrLen * (1.0 - v.lengthSq()) + edgeDotSphereVert * edgeDotSphereVert;
|
||
|
||
// Check for intersection against infinite line
|
||
var newT = getLowestRoot(a, b, c, t);
|
||
if (newT != -1 && newT < this.t) {
|
||
// Check if intersection against the line segment:
|
||
var f = (edgeDotVel * newT - edgeDotSphereVert) / edgeSqrLen;
|
||
if (f >= 0.0 && f <= 1.0) {
|
||
v = edge.multiply(f);
|
||
v = v.add(pa);
|
||
this.setCollision(newT, v);
|
||
return newT;
|
||
}
|
||
}
|
||
return t;
|
||
}
|
||
}
|