Add MPs

src/Main.hx

@@ -46,19 +46,19 @@ class Main extends hxd.App {
 
 		var m1 = new PathedInteriorMarker();
 		m1.msToNext = 5;
-		m1.position = new Vector(5, 0, 0);
+		m1.position = new Vector(0, 0, 0);
 		m1.smoothingType = "";
 		m1.rotation = new Quat();
 
 		var m2 = new PathedInteriorMarker();
 		m2.msToNext = 3;
-		m2.position = new Vector(5, 0, 5);
+		m2.position = new Vector(0, 0, 5);
 		m2.smoothingType = "";
 		m2.rotation = new Quat();
 
 		var m3 = new PathedInteriorMarker();
 		m3.msToNext = 5;
-		m3.position = new Vector(5, 0, 0);
+		m3.position = new Vector(0, 0, 0);
 		m3.smoothingType = "";
 		m3.rotation = new Quat();
 
@@ -86,14 +86,14 @@ class Main extends hxd.App {
 
 		// s3d.camera.
 
-		var marble2 = new Marble();
+		// var marble2 = new Marble();
-		world.addMarble(marble2);
+		// world.addMarble(marble2);
-		marble2.setPosition(0, 5, 5);
+		// marble2.setPosition(5, 0, 5);
 
 		var marble = new Marble();
 		marble.controllable = true;
 		world.addMarble(marble);
-		marble.setPosition(6, 0, 5);
+		marble.setPosition(0, 0, 2);
 		// marble.setPosition(-10, -5, 5);
 	}
 

src/Marble.hx

@@ -1,5 +1,6 @@
 package src;
 
+import collision.Collision;
 import dif.math.Point3F;
 import dif.math.PlaneF;
 import collision.CollisionSurface;
@@ -251,13 +252,20 @@ class Marble extends Object {
 				dir.normalize();
 				var soFar = 0.0;
 				for (k in 0...contacts.length) {
-					if (contacts[k].penetration < this._radius) {
+					if (contacts[k].contactDistance < this._radius) {
 						var timeToSeparate = 0.1;
-						var dist = contacts[k].penetration;
+						var dist = this._radius - contacts[k].contactDistance; // contacts[k].penetration;
-						var outVel = this.velocity.add(dir.multiply(soFar)).dot(contacts[k].normal);
+						var normal = contacts[k].normal;
+						var unk = normal.multiply(soFar);
+						var tickle = this.velocity.sub(contacts[k].velocity);
+						var plop = unk.add(tickle);
+						var outVel = plop.dot(normal);
+						var cancan = timeToSeparate * outVel;
 
-						if (timeToSeparate * outVel < dist) {
+						if (dist > cancan) {
-							soFar += (dist - outVel * timeToSeparate) / timeToSeparate / contacts[k].normal.dot(dir);
+							var bla = contacts[k].normal;
+							var bFac = (dist - cancan) / timeToSeparate;
+							soFar += bFac / bla.dot(dir);
 						}
 					}
 				}
@@ -278,9 +286,9 @@ class Marble extends Object {
 		var bestNormalForce = 0.0;
 		for (i in 0...contacts.length) {
 			if (contacts[i].collider == null) {
-				var normalForce = -contacts[i].normal.dot(A);
+				contacts[i].normalForce = -contacts[i].normal.dot(A);
-				if (normalForce > bestNormalForce) {
+				if (contacts[i].normalForce > bestNormalForce) {
-					bestNormalForce = normalForce;
+					bestNormalForce = contacts[i].normalForce;
 					bestSurface = i;
 				}
 			}
@@ -373,7 +381,7 @@ class Marble extends Object {
 	function testMove(velocity:Vector, position:Vector, deltaT:Float, radius:Float, testPIs:Bool, collisionWorld:CollisionWorld) {
 		var velLen = velocity.length();
 		if (velLen < 0.001)
-			return false;
+			return deltaT;
 
 		var velocityDir = velocity.normalized();
 
@@ -411,7 +419,7 @@ class Marble extends Object {
 				var surfaces = obj.octree.radiusSearch(localpos, expandedcollider.radius);
 
 				for (surf in surfaces) {
-					var surface:CollisionSurface = cast obj;
+					var surface:CollisionSurface = cast surf;
 
 					var i = 0;
 					while (i < surface.indices.length) {
@@ -655,27 +663,131 @@ class Marble extends Object {
 			contacted = true;
 		}
 
-		return true;
+		return finalT;
 	}
 
-	function advancePhysics(m:Move, dt:Float, collisionWorld:CollisionWorld) {
+	function getIntersectionTime(dt:Float, velocity:Vector, pathedInteriors:Array<PathedInterior>, collisionWorld:CollisionWorld) {
-		this.findContacts(collisionWorld, dt);
+		var expandedcollider = new SphereCollisionEntity(cast this);
-		var cmf = this.computeMoveForces(m);
+		var position = this.getAbsPos().getPosition();
-		var isCentered:Bool = cmf.result;
+		expandedcollider.transform = Matrix.T(position.x, position.y, position.z);
-		var aControl = cmf.aControl;
+		expandedcollider.radius = this.getAbsPos().getPosition().distance(position) + _radius;
-		var desiredOmega = cmf.desiredOmega;
+
-		this.velocityCancel(isCentered, false);
+		var foundObjs = collisionWorld.radiusSearch(position, expandedcollider.radius);
-		var A = this.getExternalForces(m, dt);
+
-		var retf = this.applyContactForces(dt, m, isCentered, aControl, desiredOmega, A);
+		function toDifPoint(vec:Vector) {
-		A = retf[0];
+			return new Point3F(vec.x, vec.y, vec.z);
-		var a = retf[1];
-		this.velocity = this.velocity.add(A.multiply(dt));
-		this.omega = this.omega.add(a.multiply(dt));
-		this.velocityCancel(isCentered, true);
-		this._totalTime += dt;
-		if (contacts.length != 0) {
-			this._contactTime += dt;
 		}
+
+		var intersectT = 10e8;
+
+		for (obj in foundObjs) {
+			if (obj.velocity.length() > 0) {
+				var radius = _radius;
+
+				var invMatrix = obj.transform.clone();
+				invMatrix.invert();
+				var localpos = position.clone();
+				localpos.transform(invMatrix);
+				var surfaces = obj.octree.radiusSearch(localpos, radius * 1.1);
+
+				var tform = obj.transform.clone();
+				var velDir = obj.velocity.normalized();
+				// tform.setPosition(tform.getPosition().add(velDir.multiply(_radius)));
+				tform.setPosition(tform.getPosition().add(obj.velocity.multiply(dt)).sub(velDir.multiply(_radius)));
+
+				var contacts = [];
+
+				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(tform);
+						var v = surface.points[surface.indices[i + 1]].transformed(tform);
+						var v2 = surface.points[surface.indices[i + 2]].transformed(tform);
+
+						var polyPlane = PlaneF.ThreePoints(toDifPoint(v0), toDifPoint(v), toDifPoint(v2));
+
+						var surfacenormal = surface.normals[surface.indices[i]].transformed3x3(obj.transform);
+
+						var t = (-position.dot(surfacenormal) - polyPlane.d) / velocity.dot(surfacenormal);
+
+						var pt = position.add(velocity.multiply(t));
+
+						if (Collision.PointInTriangle(pt, v0, v, v2)) {
+							if (t > 0 && t < intersectT) {
+								intersectT = t;
+							}
+						}
+
+						i += 3;
+					}
+				}
+			}
+		}
+
+		return intersectT;
+	}
+
+	function advancePhysics(currentTime:Float, dt:Float, m:Move, collisionWorld:CollisionWorld, pathedInteriors:Array<PathedInterior>) {
+		var timeRemaining = dt;
+		var it = 0;
+
+		var piTime = currentTime;
+		do {
+			if (timeRemaining <= 0)
+				break;
+
+			var timeStep = 0.00800000037997961;
+			if (timeRemaining < 0.00800000037997961)
+				timeStep = timeRemaining;
+
+			this.findContacts(collisionWorld, timeStep);
+			var cmf = this.computeMoveForces(m);
+			var isCentered:Bool = cmf.result;
+			var aControl = cmf.aControl;
+			var desiredOmega = cmf.desiredOmega;
+			this.velocityCancel(isCentered, false);
+			var A = this.getExternalForces(m, timeStep);
+			var retf = this.applyContactForces(timeStep, m, isCentered, aControl, desiredOmega, A);
+			A = retf[0];
+			var a = retf[1];
+			this.velocity = this.velocity.add(A.multiply(timeStep));
+			this.omega = this.omega.add(a.multiply(timeStep));
+			this.velocityCancel(isCentered, true);
+			this._totalTime += timeStep;
+			if (contacts.length != 0) {
+				this._contactTime += timeStep;
+			}
+
+			var intersectT = this.getIntersectionTime(timeStep, velocity, pathedInteriors, collisionWorld);
+
+			if (intersectT < timeStep) {
+				var diff = timeStep - intersectT;
+				this.velocity = this.velocity.sub(A.multiply(diff));
+				this.omega = this.omega.sub(a.multiply(diff));
+				timeStep = intersectT;
+			}
+
+			piTime += timeStep;
+			if (this.controllable) {
+				for (interior in pathedInteriors) {
+					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;
+
+			timeRemaining -= timeStep;
+			it++;
+		} while (it <= 10);
 		this.queuedContacts = [];
 	}
 
@@ -700,33 +812,62 @@ class Marble extends Object {
 			}
 		}
 
-		var timeRemaining = dt;
+		advancePhysics(currentTime, dt, move, collisionWorld, pathedInteriors);
-		var it = 0;
-		do {
-			if (timeRemaining <= 0)
-				break;
 
-			var timeStep = 0.00800000037997961;
+		// var timeRemaining = dt;
-			if (timeRemaining < 0.00800000037997961)
+		// var it = 0;
-				timeStep = timeRemaining;
 
-			advancePhysics(move, timeStep, collisionWorld);
+		// var piTime = currentTime;
-			var newPos = this.getAbsPos().getPosition().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;
 
-			timeRemaining -= timeStep;
+		// var pos = this.getAbsPos().getPosition();
-			it++;
+		// do {
-		} while (it <= 10);
+		// 	if (timeRemaining <= 0)
+		// 		break;
 
-		if (this.controllable) {
+		// 	var timeStep = 0.00800000037997961;
-			for (interior in pathedInteriors) {
+		// 	if (timeRemaining < 0.00800000037997961)
-				interior.update(currentTime, dt);
+		// 		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());
 	}

src/PathedInterior.hx

@@ -6,18 +6,29 @@ import src.Util;
 import src.PathedInteriorMarker;
 import src.InteriorGeometry;
 
-class PathedInterior extends InteriorGeometry {
+typedef PIState = {
-	public var markerData:Array<PathedInteriorMarker> = [];
-
-	var duration:Float;
 	var currentTime:Float;
 	var targetTime:Float;
 	var changeTime:Float;
-
 	var prevPosition:Vector;
 	var currentPosition:Vector;
-
 	var velocity:Vector;
+}
+
+class PathedInterior extends InteriorGeometry {
+	public var markerData:Array<PathedInteriorMarker> = [];
+
+	public var duration:Float;
+	public var currentTime:Float;
+	public var targetTime:Float;
+	public var changeTime:Float;
+
+	public var prevPosition:Vector;
+	public var currentPosition:Vector;
+
+	public var velocity:Vector;
+
+	var previousState:PIState;
 
 	public function new() {
 		super();
@@ -29,6 +40,15 @@ class PathedInterior extends InteriorGeometry {
 	}
 
 	public function update(currentTime:Float, dt:Float) {
+		this.previousState = {
+			currentTime: currentTime,
+			targetTime: targetTime,
+			changeTime: changeTime,
+			prevPosition: prevPosition,
+			currentPosition: currentPosition,
+			velocity: velocity
+		};
+
 		var transform = this.getTransformAtTime(this.getInternalTime(currentTime));
 		this.updatePosition();
 
@@ -39,6 +59,16 @@ class PathedInterior extends InteriorGeometry {
 		velocity = position.sub(this.prevPosition).multiply(1 / dt);
 	}
 
+	public function rollBack() {
+		this.currentTime = this.previousState.currentTime;
+		this.targetTime = this.previousState.targetTime;
+		this.changeTime = this.previousState.changeTime;
+		this.prevPosition = this.previousState.prevPosition;
+		this.currentPosition = this.previousState.currentPosition;
+		this.velocity = this.previousState.velocity;
+		this.updatePosition();
+	}
+
 	function computeDuration() {
 		var total = 0.0;
 		for (marker in markerData) {

src/collision/Collision.hx

@@ -216,7 +216,7 @@ class Collision {
 		return res;
 	}
 
-	private static function PointInTriangle(point:Vector, v0:Vector, v1:Vector, v2:Vector):Bool {
+	public static function PointInTriangle(point:Vector, v0:Vector, v1:Vector, v2:Vector):Bool {
 		var u = v1.sub(v0);
 		var v = v2.sub(v0);
 		var w = point.sub(v0);
@@ -242,7 +242,7 @@ class Collision {
 		return (r + t) <= 1;
 	}
 
-	private static function PointInTriangle2(point:Vector, a:Vector, b:Vector, c:Vector):Bool {
+	public static function PointInTriangle2(point:Vector, a:Vector, b:Vector, c:Vector):Bool {
 		var a1 = a.sub(point);
 		var b1 = b.sub(point);
 		var c1 = c.sub(point);

src/collision/CollisionEntity.hx

@@ -74,7 +74,7 @@ class CollisionEntity implements IOctreeObject {
 		var surfaces = octree.radiusSearch(localpos, radius * 1.1);
 
 		var tform = transform.clone();
-		// tform.setPosition(tform.getPosition().add(velocity.multiply(dt)));
+		tform.setPosition(tform.getPosition().add(this.velocity.multiply(dt)));
 
 		var contacts = [];
 
@@ -104,8 +104,10 @@ class CollisionEntity implements IOctreeObject {
 							cinfo.point = closest;
 							// cinfo.collider = this;
 							cinfo.velocity = this.velocity;
-							cinfo.penetration = radius - (position.sub(closest).dot(normal));
+							cinfo.contactDistance = closest.distance(position);
+							// cinfo.penetration = radius - (position.sub(closest).dot(normal));
 							cinfo.restitution = 1;
+							cinfo.force = 0;
 							cinfo.friction = 1;
 							contacts.push(cinfo);
 						}

src/collision/CollisionInfo.hx

@@ -8,8 +8,11 @@ class CollisionInfo {
 	public var velocity:Vector;
 	public var collider:CollisionEntity;
 	public var friction:Float;
+	public var vAtCMag:Float;
+	public var normalForce:Float;
 	public var restitution:Float;
-	public var penetration:Float;
+	public var contactDistance:Float;
+	public var force:Float;
 
 	public function new() {}
 }

src/collision/CollisionWorld.hx

@@ -53,12 +53,12 @@ class CollisionWorld {
 		for (obj in intersections) {
 			var entity:CollisionEntity = cast obj;
 
-			contacts = contacts.concat(entity);
+			contacts.push(entity);
 		}
 
 		for (obj in dynamicEntities) {
 			if (obj.boundingBox.collide(box))
-				contacts = contacts.concat(obj);
+				contacts.push(obj);
 		}
 
 		return contacts;

src/collision/SphereCollisionEntity.hx

@@ -45,7 +45,9 @@ class SphereCollisionEntity extends CollisionEntity {
 			contact.velocity = this.velocity;
 			contact.point = position.add(thispos).multiply(0.5);
 			contact.normal = contact.point.sub(thispos).normalized();
-			contact.penetration = radius - (position.sub(contact.point).dot(contact.normal));
+			contact.force = 0;
+			contact.contactDistance = contact.point.distance(position);
+			// contact.penetration = radius - (position.sub(contact.point).dot(contact.normal));
 			contacts.push(contact);
 
 			var othercontact = new CollisionInfo();
@@ -55,7 +57,9 @@ class SphereCollisionEntity extends CollisionEntity {
 			othercontact.velocity = this.velocity;
 			othercontact.point = thispos.add(position).multiply(0.5);
 			othercontact.normal = contact.point.sub(position).normalized();
-			othercontact.penetration = this.radius - (thispos.sub(othercontact.point).dot(othercontact.normal));
+			othercontact.contactDistance = contact.point.distance(position);
+			othercontact.force = 0;
+			// othercontact.penetration = this.radius - (thispos.sub(othercontact.point).dot(othercontact.normal));
 			this.marble.queueCollision(othercontact);
 		}
 		return contacts;