Robust entry collisions

- Collision separation now supports motion sweeping in order to avoid tunnelling when entities drop onto contraptions from a greater height
- Further improved the collision response
- Entities can no longer be clipped into solid walls by the collision response

src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/ContraptionCollider.java

@@ -1,20 +1,26 @@
 package com.simibubi.create.content.contraptions.components.structureMovement;
 
 import static java.util.concurrent.TimeUnit.SECONDS;
+import static net.minecraft.entity.Entity.collideBoundingBoxHeuristically;
+import static net.minecraft.entity.Entity.horizontalMag;
 
 import java.lang.ref.WeakReference;
 import java.util.ArrayList;
 import java.util.Iterator;
 import java.util.List;
 import java.util.concurrent.ExecutionException;
+import java.util.stream.Stream;
 
+import org.apache.commons.lang3.mutable.MutableBoolean;
 import org.apache.commons.lang3.mutable.MutableObject;
 
 import com.google.common.base.Predicates;
 import com.google.common.cache.Cache;
 import com.google.common.cache.CacheBuilder;
+import com.google.common.collect.ImmutableSet;
 import com.simibubi.create.AllBlocks;
 import com.simibubi.create.content.contraptions.components.actors.BlockBreakingMovementBehaviour;
+import com.simibubi.create.foundation.collision.ContinuousOBBCollider.ContinuousSeparationManifold;
 import com.simibubi.create.foundation.collision.Matrix3d;
 import com.simibubi.create.foundation.collision.OrientedBB;
 import com.simibubi.create.foundation.utility.AngleHelper;
@@ -29,7 +35,7 @@ import net.minecraft.entity.EntityType;
 import net.minecraft.entity.MoverType;
 import net.minecraft.entity.player.PlayerEntity;
 import net.minecraft.entity.player.ServerPlayerEntity;
-import net.minecraft.nbt.CompoundNBT;
+import net.minecraft.util.DamageSource;
 import net.minecraft.util.Direction;
 import net.minecraft.util.Direction.Axis;
 import net.minecraft.util.Direction.AxisDirection;
@@ -37,12 +43,14 @@ import net.minecraft.util.ReuseableStream;
 import net.minecraft.util.math.AxisAlignedBB;
 import net.minecraft.util.math.BlockPos;
 import net.minecraft.util.math.Vec3d;
+import net.minecraft.util.math.shapes.IBooleanFunction;
+import net.minecraft.util.math.shapes.ISelectionContext;
 import net.minecraft.util.math.shapes.VoxelShape;
+import net.minecraft.util.math.shapes.VoxelShapes;
 import net.minecraft.world.World;
 import net.minecraft.world.gen.feature.template.Template.BlockInfo;
 import net.minecraftforge.api.distmarker.Dist;
 import net.minecraftforge.api.distmarker.OnlyIn;
-import net.minecraftforge.common.util.Constants.NBT;
 import net.minecraftforge.event.TickEvent.ClientTickEvent;
 import net.minecraftforge.event.TickEvent.Phase;
 import net.minecraftforge.event.TickEvent.WorldTickEvent;
@@ -54,9 +62,11 @@ import net.minecraftforge.fml.common.Mod.EventBusSubscriber;
 @EventBusSubscriber
 public class ContraptionCollider {
 
+	public static DamageSource damageSourceContraptionSuffocate =
+		new DamageSource("create.contraption_suffocate").setDamageBypassesArmor();
 	public static boolean wasClientPlayerGrounded;
 	public static Cache<World, List<WeakReference<ContraptionEntity>>> activeContraptions = CacheBuilder.newBuilder()
-		.expireAfterAccess(20, SECONDS)
+		.expireAfterAccess(40, SECONDS)
 		.build();
 
 	@SubscribeEvent
@@ -86,7 +96,7 @@ public class ContraptionCollider {
 
 	@SubscribeEvent
 	public static void entityCollisionHappensPreWorldTick(WorldTickEvent event) {
-		if (event.phase == Phase.START)
+		if (event.phase == Phase.END)
 			return;
 		World world = event.world;
 		runCollisions(world);
@@ -124,15 +134,17 @@ public class ContraptionCollider {
 		double conRotY = contraptionRotation.y;
 		double conRotZ = contraptionRotation.x;
 
-		for (Entity entity : world.getEntitiesWithinAABB((EntityType<?>) null, bounds.grow(2),
+		for (Entity entity : world.getEntitiesWithinAABB((EntityType<?>) null, bounds.grow(2)
-			contraptionEntity::canCollideWith)) {
+			.expand(0, 32, 0), contraptionEntity::canCollideWith)) {
-			if (entity instanceof PlayerEntity && !world.isRemote)
+			boolean serverPlayer = entity instanceof PlayerEntity && !world.isRemote;
-				return;
 
+			// Transform entity position and motion to local space
 			Vec3d centerOfBlock = VecHelper.getCenterOf(BlockPos.ZERO);
 			Vec3d entityPosition = entity.getPositionVec();
-			Vec3d centerY = new Vec3d(0, entity.getBoundingBox()
+			AxisAlignedBB entityBounds = entity.getBoundingBox();
-				.getYSize() / 2, 0);
+			Vec3d centerY = new Vec3d(0, entityBounds.getYSize() / 2, 0);
+			Vec3d motion = entity.getMotion();
+			boolean axisAlignedCollision = contraptionRotation.equals(Vec3d.ZERO);
 
 			Vec3d position =
 				entityPosition.subtract(contraptionEntity.stationary ? centerOfBlock : Vec3d.ZERO.add(0, 0.5, 0))
@@ -143,27 +155,22 @@ public class ContraptionCollider {
 			position = position.add(centerOfBlock)
 				.subtract(centerY)
 				.subtract(entityPosition);
-			AxisAlignedBB localBB = entity.getBoundingBox()
+
-				.offset(position)
+			// Find all potential block shapes to collide with
+			AxisAlignedBB localBB = entityBounds.offset(position)
 				.grow(1.0E-7D);
-
+			ReuseableStream<VoxelShape> potentialHits =
-			String nbtMotionKey = "ContraptionCollisionFeedback";
+				getPotentiallyCollidedShapes(world, contraption, localBB.expand(motion));
-			CompoundNBT entityData = entity.getPersistentData();
-			Vec3d previousIntersection = Vec3d.ZERO;
-			if (entityData.contains(nbtMotionKey)) {
-				previousIntersection = VecHelper.readNBT(entityData.getList(nbtMotionKey, NBT.TAG_DOUBLE));
-				entity.setMotion(entity.getMotion()
-					.subtract(previousIntersection.mul(1, 0, 1)));
-				entityData.remove(nbtMotionKey);
-			}
-
-			ReuseableStream<VoxelShape> potentialHits = getPotentiallyCollidedShapes(world, contraption, localBB);
 			if (potentialHits.createStream()
 				.count() == 0)
 				continue;
 
+			if (!axisAlignedCollision)
+				motion = VecHelper.rotate(motion, -conRotX, -conRotY, -conRotZ);
+
+			// Prepare entity bounds
 			OrientedBB obb = new OrientedBB(localBB);
-			if (!contraptionRotation.equals(Vec3d.ZERO)) {
+			if (!axisAlignedCollision) {
 				Matrix3d rotation = new Matrix3d().asIdentity();
 				rotation.multiply(new Matrix3d().asXRotation(AngleHelper.rad(-conRotX)));
 				rotation.multiply(new Matrix3d().asYRotation(AngleHelper.rad(conRotY)));
@@ -171,32 +178,87 @@ public class ContraptionCollider {
 				obb.setRotation(rotation);
 			}
 
+//			Vec3d visualizerOrigin = new Vec3d(10, 64, 0);
+//			CollisionDebugger.OBB = obb.copy();
+//			CollisionDebugger.OBB.move(visualizerOrigin);
+
 			MutableObject<Vec3d> collisionResponse = new MutableObject<>(Vec3d.ZERO);
+			MutableObject<Vec3d> allowedMotion = new MutableObject<>(motion);
+			MutableBoolean futureCollision = new MutableBoolean(false);
+			MutableBoolean surfaceCollision = new MutableBoolean(false);
 			Vec3d obbCenter = obb.getCenter();
 
+			// Apply separation maths
+			List<AxisAlignedBB> bbs = new ArrayList<>();
 			potentialHits.createStream()
-				.forEach(shape -> {
+				.forEach(shape -> shape.toBoundingBoxList()
-					Vec3d currentResponse = collisionResponse.getValue();
+					.forEach(bbs::add));
-					shape.toBoundingBoxList()
-						.parallelStream()
-						.forEach(bb -> {
-							obb.setCenter(obbCenter.add(currentResponse));
-							Vec3d intersect = obb.intersect(bb);
-							if (intersect != null)
-								collisionResponse.setValue(currentResponse.add(intersect));
-						});
-				});
 
+			for (AxisAlignedBB bb : bbs) {
+				Vec3d currentResponse = collisionResponse.getValue();
+				obb.setCenter(obbCenter.add(currentResponse));
+				ContinuousSeparationManifold intersect = obb.intersect(bb, allowedMotion.getValue());
+//				OutlineParams params = CreateClient.outliner.showAABB(bb, bb.offset(visualizerOrigin))
+//					.withFaceTexture(AllSpecialTextures.HIGHLIGHT_CHECKERED);
+//				params.colored(0xffffff);
+
+				if (intersect == null)
+					continue;
+				if (surfaceCollision.isFalse())
+					surfaceCollision.setValue(intersect.isSurfaceCollision());
+
+				double timeOfImpact = intersect.getTimeOfImpact();
+				if (timeOfImpact > 0 && timeOfImpact < 1) {
+					futureCollision.setTrue();
+//					Vec3d prev = allowedMotion.getValue();
+					allowedMotion.setValue(intersect.getAllowedMotion(allowedMotion.getValue()));
+//					Debug.debugChat("Allowed Motion FROM " + prev.toString());
+//					Debug.debugChat("Allowed Motion TO " + allowedMotion.getValue()
+//						.toString());
+//					params.colored(0x4499ff);
+					continue;
+				}
+				Vec3d separation = intersect.asSeparationVec();
+				if (separation != null && !separation.equals(Vec3d.ZERO)) {
+					collisionResponse.setValue(currentResponse.add(separation));
+//					Debug.debugChat("Collision " + currentResponse.add(separation)
+//						.toString());
+//					params.colored(0xff9944);
+				}
+			}
+
+//			Debug.debugChat("----");
+
+			// Resolve collision
 			Vec3d entityMotion = entity.getMotion();
 			Vec3d totalResponse = collisionResponse.getValue();
+			Vec3d motionResponse = allowedMotion.getValue();
 
-			if (totalResponse == Vec3d.ZERO)
+			if (futureCollision.isTrue() && !serverPlayer) {
+				if (!axisAlignedCollision)
+					motionResponse = VecHelper.rotate(motionResponse, conRotX, conRotY, conRotZ);
+				if (motionResponse.y != entityMotion.y) {
+					entity.setMotion(entityMotion.mul(1, 0, 1)
+						.add(0, motionResponse.y, 0));
+					entityMotion = entity.getMotion();
+				}
+			}
+
+			if (!axisAlignedCollision)
+				totalResponse = VecHelper.rotate(totalResponse, conRotX, conRotY, conRotZ);
+
+			if (surfaceCollision.isTrue()) {
+//				entity.handleFallDamage(entity.fallDistance, 1); tunnelling issue
+				entity.fallDistance = 0;
+				entity.onGround = true;
+				if (!serverPlayer) {
+					DistExecutor.runWhenOn(Dist.CLIENT, () -> () -> checkForClientPlayerCollision(entity));
+				}
+			}
+
+			if (totalResponse.equals(Vec3d.ZERO))
 				continue;
 
-			totalResponse = VecHelper.rotate(totalResponse, conRotX, Axis.X);
-			totalResponse = VecHelper.rotate(totalResponse, conRotY, Axis.Y);
-			totalResponse = VecHelper.rotate(totalResponse, conRotZ, Axis.Z);
-
 			double motionX = entityMotion.getX();
 			double motionY = entityMotion.getY();
 			double motionZ = entityMotion.getZ();
@@ -205,7 +267,6 @@ public class ContraptionCollider {
 			double intersectZ = totalResponse.getZ();
 
 			double horizonalEpsilon = 1 / 128f;
-
 			if (motionX != 0 && Math.abs(intersectX) > horizonalEpsilon && motionX > 0 == intersectX < 0)
 				entityMotion = entityMotion.mul(0, 1, 1);
 			if (motionY != 0 && intersectY != 0 && motionY > 0 == intersectY < 0)
@@ -213,28 +274,62 @@ public class ContraptionCollider {
 			if (motionZ != 0 && Math.abs(intersectZ) > horizonalEpsilon && motionZ > 0 == intersectZ < 0)
 				entityMotion = entityMotion.mul(1, 1, 0);
 
-			entityMotion = entityMotion.add(totalResponse.mul(1, 0, 1));
-			contraptionEntity.collidingEntities.add(entity);
-			entity.velocityChanged = true;
-
-			if (totalResponse.y > 0) {
-				entity.handleFallDamage(entity.fallDistance, 1);
-				entity.fallDistance = 0;
-				entity.onGround = true;
-				DistExecutor.runWhenOn(Dist.CLIENT, () -> () -> checkForClientPlayerCollision(entity));
-			}
-
 			if (entity instanceof ServerPlayerEntity)
 				((ServerPlayerEntity) entity).connection.floatingTickCount = 0;
 
-			entity.setMotion(entityMotion);
+			if (!serverPlayer) {
-			Vec3d epos = entityPosition;
+				Vec3d allowedMovement = getAllowedMovement(totalResponse, entity);
-			entity.setPosition(epos.x, epos.y + totalResponse.y, epos.z);
+				contraptionEntity.collidingEntities.add(entity);
-			entityData.put(nbtMotionKey, VecHelper.writeNBT(totalResponse));
+				entity.velocityChanged = true;
+				entity.setPosition(entityPosition.x + allowedMovement.x, entityPosition.y + allowedMovement.y,
+					entityPosition.z + allowedMovement.z);
+				entity.setMotion(entityMotion);
+			}
 		}
 
 	}
 
+	/** From Entity#getAllowedMovement **/
+	static Vec3d getAllowedMovement(Vec3d movement, Entity e) {
+		AxisAlignedBB bb = e.getBoundingBox();
+		ISelectionContext ctx = ISelectionContext.forEntity(e);
+		World world = e.world;
+		VoxelShape voxelshape = world.getWorldBorder()
+			.getShape();
+		Stream<VoxelShape> stream =
+			VoxelShapes.compare(voxelshape, VoxelShapes.create(bb.shrink(1.0E-7D)), IBooleanFunction.AND)
+				? Stream.empty()
+				: Stream.of(voxelshape);
+		Stream<VoxelShape> stream1 = world.getEmptyCollisionShapes(e, bb.expand(movement), ImmutableSet.of());
+		ReuseableStream<VoxelShape> reuseablestream = new ReuseableStream<>(Stream.concat(stream1, stream));
+		Vec3d vec3d = movement.lengthSquared() == 0.0D ? movement
+			: collideBoundingBoxHeuristically(e, movement, bb, world, ctx, reuseablestream);
+		boolean flag = movement.x != vec3d.x;
+		boolean flag1 = movement.y != vec3d.y;
+		boolean flag2 = movement.z != vec3d.z;
+		boolean flag3 = e.onGround || flag1 && movement.y < 0.0D;
+		if (e.stepHeight > 0.0F && flag3 && (flag || flag2)) {
+			Vec3d vec3d1 = collideBoundingBoxHeuristically(e, new Vec3d(movement.x, (double) e.stepHeight, movement.z),
+				bb, world, ctx, reuseablestream);
+			Vec3d vec3d2 = collideBoundingBoxHeuristically(e, new Vec3d(0.0D, (double) e.stepHeight, 0.0D),
+				bb.expand(movement.x, 0.0D, movement.z), world, ctx, reuseablestream);
+			if (vec3d2.y < (double) e.stepHeight) {
+				Vec3d vec3d3 = collideBoundingBoxHeuristically(e, new Vec3d(movement.x, 0.0D, movement.z),
+					bb.offset(vec3d2), world, ctx, reuseablestream).add(vec3d2);
+				if (horizontalMag(vec3d3) > horizontalMag(vec3d1)) {
+					vec3d1 = vec3d3;
+				}
+			}
+
+			if (horizontalMag(vec3d1) > horizontalMag(vec3d)) {
+				return vec3d1.add(collideBoundingBoxHeuristically(e, new Vec3d(0.0D, -vec3d1.y + movement.y, 0.0D),
+					bb.offset(vec3d1), world, ctx, reuseablestream));
+			}
+		}
+
+		return vec3d;
+	}
+
 	@OnlyIn(Dist.CLIENT)
 	private static void checkForClientPlayerCollision(Entity entity) {
 		if (entity != Minecraft.getInstance().player)
@@ -310,7 +405,7 @@ public class ContraptionCollider {
 		double width = localBB.getXSize();
 		double horizontalFactor = (height > width && width != 0) ? height / width : 1;
 		double verticalFactor = (width > height && height != 0) ? width / height : 1;
-		AxisAlignedBB blockScanBB = localBB.grow(.5f);
+		AxisAlignedBB blockScanBB = localBB.grow(0.5f);
 		blockScanBB = blockScanBB.grow(horizontalFactor, verticalFactor, horizontalFactor);
 
 		BlockPos min = new BlockPos(blockScanBB.minX, blockScanBB.minY, blockScanBB.minZ);

src/main/java/com/simibubi/create/foundation/collision/CollisionDebugger.java

@@ -3,30 +3,35 @@ package com.simibubi.create.foundation.collision;
 import com.mojang.blaze3d.matrix.MatrixStack;
 import com.simibubi.create.AllSpecialTextures;
 import com.simibubi.create.CreateClient;
+import com.simibubi.create.foundation.collision.ContinuousOBBCollider.ContinuousSeparationManifold;
 import com.simibubi.create.foundation.renderState.SuperRenderTypeBuffer;
+import com.simibubi.create.foundation.utility.AngleHelper;
 import com.simibubi.create.foundation.utility.MatrixStacker;
 import com.simibubi.create.foundation.utility.outliner.AABBOutline;
 
+import net.minecraft.client.Minecraft;
 import net.minecraft.util.math.AxisAlignedBB;
+import net.minecraft.util.math.BlockPos;
+import net.minecraft.util.math.BlockRayTraceResult;
+import net.minecraft.util.math.RayTraceResult;
+import net.minecraft.util.math.RayTraceResult.Type;
 import net.minecraft.util.math.Vec3d;
 
 public class CollisionDebugger {
 
-	public static AxisAlignedBB AABB = null;
+	public static AxisAlignedBB AABB = new AxisAlignedBB(BlockPos.ZERO.up(10));
-	public static OrientedBB OBB = null;
+	public static OrientedBB OBB = new OrientedBB(new AxisAlignedBB(BlockPos.ZERO));
-	static Vec3d seperation;
+	public static Vec3d motion = Vec3d.ZERO;
+	static ContinuousSeparationManifold seperation;
 	static double angle = 0;
 	static AABBOutline outline;
 
 	public static void onScroll(double delta) {
-//		angle += delta;
+		angle += delta;
-//		movingBB = new OrientedBB(new AxisAlignedBB(BlockPos.ZERO).expand(0, 1, 0));
+		OBB.setRotation(new Matrix3d().asZRotation(AngleHelper.rad(angle)));
-//		movingBB.setRotation(new Matrix3d().asZRotation(AngleHelper.rad(angle)));
 	}
 
 	public static void render(MatrixStack ms, SuperRenderTypeBuffer buffer) {
-		if (OBB == null)
-			return;
 		ms.push();
 		outline = new AABBOutline(OBB.getAsAxisAlignedBB());
 		outline.getParams()
@@ -47,12 +52,12 @@ public class CollisionDebugger {
 		ms.pop();
 
 		ms.push();
-		if (seperation != null) {
+		if (motion.length() != 0 && (seperation == null || seperation.getTimeOfImpact() != 1)) {
 			outline.getParams()
-				.colored(0x65ff44)
+				.colored(0x6544ff)
 				.lineWidth(1 / 32f);
 			MatrixStacker.of(ms)
-				.translate(seperation)
+				.translate(seperation != null ? seperation.getAllowedMotion(motion) : motion)
 				.translate(OBB.center);
 			ms.peek()
 				.getModel()
@@ -62,16 +67,47 @@ public class CollisionDebugger {
 			outline.render(ms, buffer);
 		}
 		ms.pop();
+
+		ms.push();
+		if (seperation != null) {
+			Vec3d asSeparationVec = seperation.asSeparationVec();
+			if (asSeparationVec != null) {
+				outline.getParams()
+					.colored(0x65ff44)
+					.lineWidth(1 / 32f);
+				MatrixStacker.of(ms)
+					.translate(asSeparationVec)
+					.translate(OBB.center);
+				ms.peek()
+					.getModel()
+					.multiply(OBB.rotation.getAsMatrix4f());
+				MatrixStacker.of(ms)
+					.translateBack(OBB.center);
+				outline.render(ms, buffer);
+			}
+		}
+		ms.pop();
 	}
 
 	public static void tick() {
-		if (OBB == null)
+		AABB = new AxisAlignedBB(BlockPos.ZERO.up(60)).offset(.5, 0, .5);
-			return;
+		motion = new Vec3d(0, -2, -.5f);
-		if (AABB == null)
+		RayTraceResult mouse = Minecraft.getInstance().objectMouseOver;
-			return;
+		if (mouse != null && mouse.getType() == Type.BLOCK) {
-		seperation = OBB.intersect(AABB);
+			BlockRayTraceResult hit = (BlockRayTraceResult) mouse;
+			OBB.setCenter(hit.getHitVec());
+			seperation = OBB.intersect(AABB, motion);
+		}
 		CreateClient.outliner.showAABB(AABB, AABB)
 			.withFaceTexture(seperation == null ? AllSpecialTextures.CHECKERED : null);
 	}
 
+	static void showDebugLine(Vec3d relativeStart, Vec3d relativeEnd, int color, String id, int offset) {
+		Vec3d center = CollisionDebugger.AABB.getCenter()
+			.add(0, 1 + offset / 16f, 0);
+		CreateClient.outliner.showLine(id + OBBCollider.checkCount, center.add(relativeStart), center.add(relativeEnd))
+			.colored(color)
+			.lineWidth(1 / 32f);
+	}
+
 }

src/main/java/com/simibubi/create/foundation/collision/ContinuousOBBCollider.java

@@ -0,0 +1,146 @@
+package com.simibubi.create.foundation.collision;
+
+import static java.lang.Math.abs;
+import static java.lang.Math.signum;
+
+import net.minecraft.util.math.Vec3d;
+
+public class ContinuousOBBCollider extends OBBCollider {
+
+	public static ContinuousSeparationManifold separateBBs(Vec3d cA, Vec3d cB, Vec3d eA, Vec3d eB, Matrix3d m,
+		Vec3d motion) {
+		ContinuousSeparationManifold mf = new ContinuousSeparationManifold();
+
+		Vec3d diff = cB.subtract(cA);
+
+		m.transpose();
+		Vec3d diff2 = m.transform(diff);
+		Vec3d motion2 = m.transform(motion);
+		m.transpose();
+
+		double a00 = abs(m.m00);
+		double a01 = abs(m.m01);
+		double a02 = abs(m.m02);
+		double a10 = abs(m.m10);
+		double a11 = abs(m.m11);
+		double a12 = abs(m.m12);
+		double a20 = abs(m.m20);
+		double a21 = abs(m.m21);
+		double a22 = abs(m.m22);
+
+		Vec3d uB0 = new Vec3d(m.m00, m.m10, m.m20);
+		Vec3d uB1 = new Vec3d(m.m01, m.m11, m.m21);
+		Vec3d uB2 = new Vec3d(m.m02, m.m12, m.m22);
+
+		checkCount = 0;
+
+		if (
+		// Separate along A's local axes (global XYZ)
+		!(separate(mf, uA0, diff.x, eA.x, a00 * eB.x + a01 * eB.y + a02 * eB.z, motion.x)
+			|| separate(mf, uA1, diff.y, eA.y, a10 * eB.x + a11 * eB.y + a12 * eB.z, motion.y)
+			|| separate(mf, uA2, diff.z, eA.z, a20 * eB.x + a21 * eB.y + a22 * eB.z, motion.z)
+
+			// Separate along B's local axes
+			|| separate(mf, uB0, diff2.x, eA.x * a00 + eA.y * a10 + eA.z * a20, eB.x, motion2.x)
+			|| separate(mf, uB1, diff2.y, eA.x * a01 + eA.y * a11 + eA.z * a21, eB.y, motion2.y)
+			|| separate(mf, uB2, diff2.z, eA.x * a02 + eA.y * a12 + eA.z * a22, eB.z, motion2.z)))
+			return mf;
+
+		return null;
+	}
+
+	static boolean separate(ContinuousSeparationManifold mf, Vec3d axis, double TL, double rA, double rB,
+		double projectedMotion) {
+		checkCount++;
+		double distance = abs(TL);
+		double diff = distance - (rA + rB);
+
+		boolean discreteCollision = diff <= 0;
+		if (!discreteCollision && signum(projectedMotion) == signum(TL))
+			return true;
+
+		double sTL = signum(TL);
+		double value = sTL * abs(diff);
+
+		double entryTime = 0;
+		double exitTime = Double.MAX_VALUE;
+		if (!discreteCollision) {
+			mf.isDiscreteCollision = false;
+
+			if (abs(value) > abs(projectedMotion))
+				return true;
+
+			entryTime = abs(value) / abs(projectedMotion);
+			exitTime = (diff + abs(rA) + abs(rB)) / abs(projectedMotion);
+			mf.latestCollisionEntryTime = Math.max(entryTime, mf.latestCollisionEntryTime);
+			mf.earliestCollisionExitTime = Math.min(exitTime, mf.earliestCollisionExitTime);
+		}
+
+		boolean isBestSeperation = distance != 0 && -(diff) <= abs(mf.separation);
+//		boolean isBestSeperation = discreteCollision && checkCount == 5; // Debug specific separations
+
+		if (isBestSeperation) {
+
+			mf.axis = axis.normalize();
+			mf.separation = value;
+
+			// Visualize values
+//			if (CollisionDebugger.AABB != null) {
+//				Vec3d normalizedAxis = axis.normalize();
+//				showDebugLine(Vec3d.ZERO, normalizedAxis.scale(projectedMotion), 0x111155, "motion", 5);
+//				showDebugLine(Vec3d.ZERO, normalizedAxis.scale(TL), 0xbb00bb, "tl", 4);
+//				showDebugLine(Vec3d.ZERO, normalizedAxis.scale(sTL * rA), 0xff4444, "ra", 3);
+//				showDebugLine(normalizedAxis.scale(sTL * rA),
+//					normalizedAxis.scale(sTL * rA - entryTime * projectedMotion), 0x44ff44, "entry", 0);
+//				showDebugLine(normalizedAxis.scale(sTL * rA - entryTime * projectedMotion),
+//					normalizedAxis.scale(sTL * rA - entryTime * projectedMotion + exitTime * projectedMotion), 0x44ffff,
+//					"exit", -1);
+//				showDebugLine(normalizedAxis.scale(sTL * (distance - rB)), normalizedAxis.scale(TL), 0x4444ff, "rb", 2);
+//				showDebugLine(normalizedAxis.scale(sTL * (distance - rB)),
+//					normalizedAxis.scale(sTL * (distance - rB) + value), 0xff9966, "separation", 1);
+////				System.out.println("TL:" + TL + ", rA: " + rA + ", rB: " + rB);
+//			}
+
+		}
+
+		return false;
+	}
+
+	public static class ContinuousSeparationManifold extends SeparationManifold {
+
+		static final double UNDEFINED = -1;
+		double latestCollisionEntryTime = UNDEFINED;
+		double earliestCollisionExitTime = Double.MAX_VALUE;
+		boolean isDiscreteCollision = true;
+
+		public double getTimeOfImpact() {
+			if (latestCollisionEntryTime == UNDEFINED)
+				return UNDEFINED;
+			if (latestCollisionEntryTime > earliestCollisionExitTime)
+				return UNDEFINED;
+			return latestCollisionEntryTime;
+		}
+
+		public boolean isSurfaceCollision() {
+			return true;
+		}
+
+		public Vec3d getAllowedMotion(Vec3d motion) {
+			double length = motion.length();
+			return motion.normalize()
+				.scale(getTimeOfImpact() * length);
+		}
+
+		@Override
+		public Vec3d asSeparationVec() {
+			if (isDiscreteCollision)
+				return super.asSeparationVec();
+			double t = getTimeOfImpact();
+			if (t == UNDEFINED)
+				return null;
+			return Vec3d.ZERO;
+		}
+
+	}
+
+}

src/main/java/com/simibubi/create/foundation/collision/Matrix3d.java

@@ -20,6 +20,9 @@ public class Matrix3d {
 
 	public Matrix3d asXRotation(float radians) {
 		asIdentity();
+		if (radians == 0)
+			return this;
+		
 		double s = MathHelper.sin(radians);
 		double c = MathHelper.cos(radians);
 		m22 = m11 = c;
@@ -30,6 +33,9 @@ public class Matrix3d {
 
 	public Matrix3d asYRotation(float radians) {
 		asIdentity();
+		if (radians == 0)
+			return this;
+		
 		double s = MathHelper.sin(radians);
 		double c = MathHelper.cos(radians);
 		m00 = m22 = c;
@@ -40,6 +46,9 @@ public class Matrix3d {
 
 	public Matrix3d asZRotation(float radians) {
 		asIdentity();
+		if (radians == 0)
+			return this;
+		
 		double s = MathHelper.sin(radians);
 		double c = MathHelper.cos(radians);
 		m00 = m11 = c;
@@ -104,12 +113,9 @@ public class Matrix3d {
 	}
 
 	public Vec3d transform(Vec3d vec) {
-		double x = vec.x;
+		double x = vec.x * m00 + vec.y * m01 + vec.z * m02;
-		double y = vec.y;
+		double y = vec.x * m10 + vec.y * m11 + vec.z * m12;
-		double z = vec.z;
+		double z = vec.x * m20 + vec.y * m21 + vec.z * m22;
-		x = x * m00 + y * m01 + z * m02;
-		y = x * m10 + y * m11 + z * m12;
-		z = x * m20 + y * m21 + z * m22;
 		return new Vec3d(x, y, z);
 	}
 

src/main/java/com/simibubi/create/foundation/collision/OBBCollider.java

@@ -0,0 +1,103 @@
+package com.simibubi.create.foundation.collision;
+
+import static com.simibubi.create.foundation.collision.CollisionDebugger.showDebugLine;
+import static java.lang.Math.abs;
+import static java.lang.Math.signum;
+
+import net.minecraft.util.math.Vec3d;
+
+public class OBBCollider {
+
+	static final Vec3d uA0 = new Vec3d(1, 0, 0);
+	static final Vec3d uA1 = new Vec3d(0, 1, 0);
+	static final Vec3d uA2 = new Vec3d(0, 0, 1);
+
+	public static Vec3d separateBBs(Vec3d cA, Vec3d cB, Vec3d eA, Vec3d eB, Matrix3d m) {
+		SeparationManifold mf = new SeparationManifold();
+
+		Vec3d t = cB.subtract(cA);
+
+		double a00 = abs(m.m00);
+		double a01 = abs(m.m01);
+		double a02 = abs(m.m02);
+		double a10 = abs(m.m10);
+		double a11 = abs(m.m11);
+		double a12 = abs(m.m12);
+		double a20 = abs(m.m20);
+		double a21 = abs(m.m21);
+		double a22 = abs(m.m22);
+
+		Vec3d uB0 = new Vec3d(m.m00, m.m10, m.m20);
+		Vec3d uB1 = new Vec3d(m.m01, m.m11, m.m21);
+		Vec3d uB2 = new Vec3d(m.m02, m.m12, m.m22);
+
+		checkCount = 0;
+
+		if (
+		// Separate along A's local axes (global XYZ)
+		!(isSeparatedAlong(mf, uA0, t.x, eA.x, a00 * eB.x + a01 * eB.y + a02 * eB.z)
+			|| isSeparatedAlong(mf, uA1, t.y, eA.y, a10 * eB.x + a11 * eB.y + a12 * eB.z)
+			|| isSeparatedAlong(mf, uA2, t.z, eA.z, a20 * eB.x + a21 * eB.y + a22 * eB.z)
+
+			// Separate along B's local axes
+			|| isSeparatedAlong(mf, uB0, (t.x * m.m00 + t.y * m.m10 + t.z * m.m20),
+				eA.x * a00 + eA.y * a10 + eA.z * a20, eB.x)
+			|| isSeparatedAlong(mf, uB1, (t.x * m.m01 + t.y * m.m11 + t.z * m.m21),
+				eA.x * a01 + eA.y * a11 + eA.z * a21, eB.y)
+			|| isSeparatedAlong(mf, uB2, (t.x * m.m02 + t.y * m.m12 + t.z * m.m22),
+				eA.x * a02 + eA.y * a12 + eA.z * a22, eB.z)))
+			return mf.asSeparationVec();
+
+		return null;
+	}
+
+	static int checkCount = 0;
+
+	static boolean isSeparatedAlong(SeparationManifold mf, Vec3d axis, double TL, double rA, double rB) {
+		checkCount++;
+		double distance = abs(TL);
+		double diff = distance - (rA + rB);
+		if (diff > 0)
+			return true;
+
+//		boolean isBestSeperation = distance != 0 && -(diff) <= abs(bestSeparation.getValue());
+		boolean isBestSeperation = checkCount == 2; // Debug specific separations
+
+		if (isBestSeperation) {
+			double sTL = signum(TL);
+			double value = sTL * abs(diff);
+			mf.axis = axis.normalize();
+			mf.separation = value;
+
+			// Visualize values
+			if (CollisionDebugger.AABB != null) {
+				Vec3d normalizedAxis = axis.normalize();
+				showDebugLine(Vec3d.ZERO, normalizedAxis.scale(TL), 0xbb00bb, "tl", 4);
+				showDebugLine(Vec3d.ZERO, normalizedAxis.scale(sTL * rA), 0xff4444, "ra", 3);
+				showDebugLine(normalizedAxis.scale(sTL * (distance - rB)), normalizedAxis.scale(TL), 0x4444ff, "rb", 2);
+				showDebugLine(normalizedAxis.scale(sTL * (distance - rB)),
+					normalizedAxis.scale(sTL * (distance - rB) + value), 0xff9966, "separation", 1);
+				System.out.println("TL:" + TL + ", rA: " + rA + ", rB: " + rB);
+			}
+		}
+
+		return false;
+	}
+
+	static class SeparationManifold {
+		Vec3d axis;
+		double separation;
+
+		public SeparationManifold() {
+			axis = Vec3d.ZERO;
+			separation = Double.MAX_VALUE;
+		}
+
+		public Vec3d asSeparationVec() {
+			double sep = separation;
+			return axis.normalize()
+				.scale(signum(sep) * (abs(sep) + 1E-4));
+		}
+	}
+
+}

src/main/java/com/simibubi/create/foundation/collision/OrientedBB.java

@@ -1,11 +1,6 @@
 package com.simibubi.create.foundation.collision;
 
-import static java.lang.Math.abs;
+import com.simibubi.create.foundation.collision.ContinuousOBBCollider.ContinuousSeparationManifold;
-
-import org.apache.commons.lang3.mutable.MutableDouble;
-import org.apache.commons.lang3.mutable.MutableObject;
-
-import com.simibubi.create.CreateClient;
 
 import net.minecraft.util.math.AxisAlignedBB;
 import net.minecraft.util.math.Vec3d;
@@ -29,140 +24,26 @@ public class OrientedBB {
 		this.extents = extents;
 		this.setRotation(rotation);
 	}
-	
+
 	public OrientedBB copy() {
 		return new OrientedBB(center, extents, rotation);
 	}
 
 	public Vec3d intersect(AxisAlignedBB bb) {
 		Vec3d extentsA = extentsFromBB(bb);
-		Vec3d intersects = separateBBs(bb.getCenter(), center, extentsA, extents, rotation);
+		Vec3d intersects = OBBCollider.separateBBs(bb.getCenter(), center, extentsA, extents, rotation);
 		return intersects;
 	}
 
+	public ContinuousSeparationManifold intersect(AxisAlignedBB bb, Vec3d motion) {
+		Vec3d extentsA = extentsFromBB(bb);
+		return ContinuousOBBCollider.separateBBs(bb.getCenter(), center, extentsA, extents, rotation, motion);
+	}
+
 	private static Vec3d extentsFromBB(AxisAlignedBB bb) {
 		return new Vec3d(bb.getXSize() / 2, bb.getYSize() / 2, bb.getZSize() / 2);
 	}
 
-	public static Vec3d separateBBs(Vec3d cA, Vec3d cB, Vec3d eA, Vec3d eB, Matrix3d m) {
-		Vec3d t = cB.subtract(cA);
-		double a00 = abs(m.m00);
-		double a01 = abs(m.m01);
-		double a02 = abs(m.m02);
-		double a10 = abs(m.m10);
-		double a11 = abs(m.m11);
-		double a12 = abs(m.m12);
-		double a20 = abs(m.m20);
-		double a21 = abs(m.m21);
-		double a22 = abs(m.m22);
-
-		MutableObject<Vec3d> bestAxis = new MutableObject<>(Vec3d.ZERO);
-		MutableDouble bestSep = new MutableDouble(Double.MAX_VALUE);
-
-		Vec3d uA0 = new Vec3d(1, 0, 0);
-		Vec3d uA1 = new Vec3d(0, 1, 0);
-		Vec3d uA2 = new Vec3d(0, 0, 1);
-
-		Vec3d uB0 = new Vec3d(m.m00, m.m10, m.m20);
-		Vec3d uB1 = new Vec3d(m.m01, m.m11, m.m21);
-		Vec3d uB2 = new Vec3d(m.m02, m.m12, m.m22);
-
-		checkCount = 0;
-
-		if (
-
-		// Separate along A's local axes (global XYZ)
-		!(isSeparatedAlong(bestAxis, bestSep, uA0, t.x, eA.x, a00 * eB.x + a01 * eB.y + a02 * eB.z)
-			|| isSeparatedAlong(bestAxis, bestSep, uA1, t.y, eA.y, a10 * eB.x + a11 * eB.y + a12 * eB.z)
-			|| isSeparatedAlong(bestAxis, bestSep, uA2, t.z, eA.z, a20 * eB.x + a21 * eB.y + a22 * eB.z)
-
-			// Separate along B's local axes
-			|| isSeparatedAlong(bestAxis, bestSep, uB0, (t.x * m.m00 + t.y * m.m10 + t.z * m.m20),
-				eA.x * a00 + eA.y * a10 + eA.z * a20, eB.x)
-			|| isSeparatedAlong(bestAxis, bestSep, uB1, (t.x * m.m01 + t.y * m.m11 + t.z * m.m21),
-				eA.x * a01 + eA.y * a11 + eA.z * a21, eB.y)
-			|| isSeparatedAlong(bestAxis, bestSep, uB2, (t.x * m.m02 + t.y * m.m12 + t.z * m.m22),
-				eA.x * a02 + eA.y * a12 + eA.z * a22, eB.z)
-
-		/*
-		 * The following checks (edge-to-edge) need special separation logic. They are
-		 * not necessary as long as the obb is only rotated around one axis at a time
-		 * (Which is the case for contraptions at the moment)
-		 *
-		 */
-
-		// Separate along axes perpendicular to AxB
-//			|| isSeparatedAlong(bestAxis, bestSep, uA0.crossProduct(uB0), t.z * m.m10 - t.y * m.m20,
-//				eA.y * a20 + eA.z * a10, eB.y * a02 + eB.z * a01)
-//			|| isSeparatedAlong(bestAxis, bestSep, uA0.crossProduct(uB1), t.z * m.m11 - t.y * m.m21,
-//				eA.y * a21 + eA.z * a11, eB.x * a02 + eB.z * a00)
-//			|| isSeparatedAlong(bestAxis, bestSep, uA0.crossProduct(uB2), t.z * m.m12 - t.y * m.m22,
-//				eA.y * a22 + eA.z * a12, eB.x * a01 + eB.y * a00)
-//
-//			|| isSeparatedAlong(bestAxis, bestSep, uA1.crossProduct(uB0), t.x * m.m20 - t.z * m.m00,
-//				eA.x * a20 + eA.z * a00, eB.y * a12 + eB.z * a11)
-//			|| isSeparatedAlong(bestAxis, bestSep, uA1.crossProduct(uB1), t.x * m.m21 - t.z * m.m01,
-//				eA.x * a21 + eA.z * a01, eB.x * a12 + eB.z * a10)
-//			|| isSeparatedAlong(bestAxis, bestSep, uA1.crossProduct(uB2), t.x * m.m22 - t.z * m.m02,
-//				eA.x * a22 + eA.z * a02, eB.x * a11 + eB.y * a10)
-//
-//			|| isSeparatedAlong(bestAxis, bestSep, uA2.crossProduct(uB0), t.y * m.m00 - t.x * m.m10,
-//				eA.x * a10 + eA.y * a00, eB.y * a22 + eB.z * a21)
-//			|| isSeparatedAlong(bestAxis, bestSep, uA2.crossProduct(uB1), t.y * m.m01 - t.x * m.m11,
-//				eA.x * a11 + eA.y * a01, eB.x * a22 + eB.z * a20)
-//			|| isSeparatedAlong(bestAxis, bestSep, uA2.crossProduct(uB2), t.y * m.m02 - t.x * m.m12,
-//				eA.x * a12 + eA.y * a02, eB.x * a21 + eB.y * a20)
-		))
-
-			return bestAxis.getValue()
-				.normalize()
-				.scale(bestSep.getValue());
-
-		return null;
-	}
-
-	static int checkCount = 0;
-
-	static boolean isSeparatedAlong(MutableObject<Vec3d> bestAxis, MutableDouble bestSeparation, Vec3d axis, double TL,
-		double rA, double rB) {
-		double distance = abs(TL);
-
-		checkCount++;
-
-		double diff = distance - (rA + rB);
-		if (diff > 0)
-			return true;
-		boolean isBestSeperation = distance != 0 && -(diff) <= abs(bestSeparation.getValue());
-//		boolean isBestSeperation = checkCount == 12; // Debug specific separations
-		if (isBestSeperation) {
-			bestAxis.setValue(axis.normalize());
-			double sTL = Math.signum(TL);
-			double value = sTL * abs(diff);
-			bestSeparation.setValue(value);
-
-			// Visualize values
-//			if (CollisionDebugger.staticBB != null) {
-//				Vec3d normalizedAxis = axis.normalize();
-//				showDebugLine(Vec3d.ZERO, normalizedAxis.scale(TL), 0xbb00bb, "tl", 4);
-//				showDebugLine(Vec3d.ZERO, normalizedAxis.scale(sTL * rA), 0xff4444, "ra", 3);
-//				showDebugLine(normalizedAxis.scale(sTL * (distance - rB)), normalizedAxis.scale(TL), 0x4444ff, "rb", 2);
-//				showDebugLine(normalizedAxis.scale(sTL * (distance - rB)),
-//					normalizedAxis.scale(sTL * (distance - rB) + value), 0xff9966, "separation", 1);
-//				System.out.println("TL:" + TL + ", rA: " + rA + ", rB: " + rB);
-//			}
-		}
-
-		return false;
-	}
-
-	static void showDebugLine(Vec3d relativeStart, Vec3d relativeEnd, int color, String id, int offset) {
-		Vec3d center = CollisionDebugger.AABB.getCenter()
-			.add(0, 1 + offset / 16f, 0);
-		CreateClient.outliner.showLine(id + checkCount, center.add(relativeStart), center.add(relativeEnd))
-			.colored(color)
-			.lineWidth(1 / 32f);
-	}
-
 	public Matrix3d getRotation() {
 		return rotation;
 	}
@@ -178,7 +59,7 @@ public class OrientedBB {
 	public void setCenter(Vec3d center) {
 		this.center = center;
 	}
-	
+
 	public void move(Vec3d offset) {
 		setCenter(getCenter().add(offset));
 	}
@@ -188,4 +69,33 @@ public class OrientedBB {
 			.grow(extents.x, extents.y, extents.z);
 	}
 
+	/*
+	 * The following checks (edge-to-edge) need special separation logic. They are
+	 * not necessary as long as the obb is only rotated around one axis at a time
+	 * (Which is the case for contraptions at the moment)
+	 *
+	 */
+
+	// Separate along axes perpendicular to AxB
+//		|| isSeparatedAlong(bestAxis, bestSep, uA0.crossProduct(uB0), t.z * m.m10 - t.y * m.m20,
+//			eA.y * a20 + eA.z * a10, eB.y * a02 + eB.z * a01)
+//		|| isSeparatedAlong(bestAxis, bestSep, uA0.crossProduct(uB1), t.z * m.m11 - t.y * m.m21,
+//			eA.y * a21 + eA.z * a11, eB.x * a02 + eB.z * a00)
+//		|| isSeparatedAlong(bestAxis, bestSep, uA0.crossProduct(uB2), t.z * m.m12 - t.y * m.m22,
+//			eA.y * a22 + eA.z * a12, eB.x * a01 + eB.y * a00)
+//
+//		|| isSeparatedAlong(bestAxis, bestSep, uA1.crossProduct(uB0), t.x * m.m20 - t.z * m.m00,
+//			eA.x * a20 + eA.z * a00, eB.y * a12 + eB.z * a11)
+//		|| isSeparatedAlong(bestAxis, bestSep, uA1.crossProduct(uB1), t.x * m.m21 - t.z * m.m01,
+//			eA.x * a21 + eA.z * a01, eB.x * a12 + eB.z * a10)
+//		|| isSeparatedAlong(bestAxis, bestSep, uA1.crossProduct(uB2), t.x * m.m22 - t.z * m.m02,
+//			eA.x * a22 + eA.z * a02, eB.x * a11 + eB.y * a10)
+//
+//		|| isSeparatedAlong(bestAxis, bestSep, uA2.crossProduct(uB0), t.y * m.m00 - t.x * m.m10,
+//			eA.x * a10 + eA.y * a00, eB.y * a22 + eB.z * a21)
+//		|| isSeparatedAlong(bestAxis, bestSep, uA2.crossProduct(uB1), t.y * m.m01 - t.x * m.m11,
+//			eA.x * a11 + eA.y * a01, eB.x * a22 + eB.z * a20)
+//		|| isSeparatedAlong(bestAxis, bestSep, uA2.crossProduct(uB2), t.y * m.m02 - t.x * m.m12,
+//			eA.x * a12 + eA.y * a02, eB.x * a21 + eB.y * a20)
+
 }

src/main/java/com/simibubi/create/foundation/utility/AngleHelper.java

@@ -28,10 +28,14 @@ public class AngleHelper {
 	}
 
 	public static float rad(double angle) {
+		if (angle == 0)
+			return 0;
 		return (float) (angle / 180 * Math.PI);
 	}
 
 	public static float deg(double angle) {
+		if (angle == 0)
+			return 0;
 		return (float) (angle * 180 / Math.PI);
 	}