Projected stepheight

- The oriented collision response now accounts for an entities' step height to automatically climb blocks such as stairs or slabs

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

@@ -222,7 +222,7 @@ public class ContraptionCollider {
 //					params.colored(0x4499ff);
 					continue;
 				}
-				Vec3d separation = intersect.asSeparationVec();
+				Vec3d separation = intersect.asSeparationVec(entity.stepHeight);
 				if (separation != null && !separation.equals(Vec3d.ZERO)) {
 					collisionResponse.setValue(currentResponse.add(separation));
 //					Debug.debugChat("Collision " + currentResponse.add(separation)

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

@@ -6,6 +6,7 @@ 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.Debug;
 import com.simibubi.create.foundation.utility.MatrixStacker;
 import com.simibubi.create.foundation.utility.outliner.AABBOutline;
 
@@ -28,7 +29,9 @@ public class CollisionDebugger {
 
 	public static void onScroll(double delta) {
 		angle += delta;
+		angle = (int) angle;
 		OBB.setRotation(new Matrix3d().asZRotation(AngleHelper.rad(angle)));
+		Debug.debugMessage("Angle: " + angle);
 	}
 
 	public static void render(MatrixStack ms, SuperRenderTypeBuffer buffer) {
@@ -51,26 +54,26 @@ public class CollisionDebugger {
 		outline.render(ms, buffer);
 		ms.pop();
 
-		ms.push();
-		if (motion.length() != 0 && (seperation == null || seperation.getTimeOfImpact() != 1)) {
-			outline.getParams()
-				.colored(0x6544ff)
-				.lineWidth(1 / 32f);
-			MatrixStacker.of(ms)
-				.translate(seperation != null ? seperation.getAllowedMotion(motion) : motion)
-				.translate(OBB.center);
-			ms.peek()
-				.getModel()
-				.multiply(OBB.rotation.getAsMatrix4f());
-			MatrixStacker.of(ms)
-				.translateBack(OBB.center);
-			outline.render(ms, buffer);
-		}
-		ms.pop();
+//		ms.push();
+//		if (motion.length() != 0 && (seperation == null || seperation.getTimeOfImpact() != 1)) {
+//			outline.getParams()
+//				.colored(0x6544ff)
+//				.lineWidth(1 / 32f);
+//			MatrixStacker.of(ms)
+//				.translate(seperation != null ? seperation.getAllowedMotion(motion) : motion)
+//				.translate(OBB.center);
+//			ms.peek()
+//				.getModel()
+//				.multiply(OBB.rotation.getAsMatrix4f());
+//			MatrixStacker.of(ms)
+//				.translateBack(OBB.center);
+//			outline.render(ms, buffer);
+//		}
+//		ms.pop();
 
 		ms.push();
 		if (seperation != null) {
-			Vec3d asSeparationVec = seperation.asSeparationVec();
+			Vec3d asSeparationVec = seperation.asSeparationVec(.5f);
 			if (asSeparationVec != null) {
 				outline.getParams()
 					.colored(0x65ff44)
@@ -91,7 +94,7 @@ public class CollisionDebugger {
 
 	public static void tick() {
 		AABB = new AxisAlignedBB(BlockPos.ZERO.up(60)).offset(.5, 0, .5);
-		motion = new Vec3d(0, -2, -.5f);
+		motion = Vec3d.ZERO;
 		RayTraceResult mouse = Minecraft.getInstance().objectMouseOver;
 		if (mouse != null && mouse.getType() == Type.BLOCK) {
 			BlockRayTraceResult hit = (BlockRayTraceResult) mouse;

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

@@ -33,6 +33,8 @@ public class ContinuousOBBCollider extends OBBCollider {
 		Vec3d uB2 = new Vec3d(m.m02, m.m12, m.m22);
 
 		checkCount = 0;
+		mf.stepSeparationAxis = uB1;
+		mf.stepSeparation = Double.MAX_VALUE;
 
 		if (
 		// Separate along A's local axes (global XYZ)
@@ -60,29 +62,62 @@ public class ContinuousOBBCollider extends OBBCollider {
 			return true;
 
 		double sTL = signum(TL);
-		double value = sTL * abs(diff);
+		double seperation = sTL * abs(diff);
 
 		double entryTime = 0;
 		double exitTime = Double.MAX_VALUE;
 		if (!discreteCollision) {
 			mf.isDiscreteCollision = false;
 
-			if (abs(value) > abs(projectedMotion))
+			if (abs(seperation) > abs(projectedMotion))
 				return true;
 
-			entryTime = abs(value) / abs(projectedMotion);
+			entryTime = abs(seperation) / abs(projectedMotion);
 			exitTime = (diff + abs(rA) + abs(rB)) / abs(projectedMotion);
 			mf.latestCollisionEntryTime = Math.max(entryTime, mf.latestCollisionEntryTime);
 			mf.earliestCollisionExitTime = Math.min(exitTime, mf.earliestCollisionExitTime);
 		}
 
+		Vec3d normalizedAxis = axis.normalize();
+
 		boolean isBestSeperation = distance != 0 && -(diff) <= abs(mf.separation);
 //		boolean isBestSeperation = discreteCollision && checkCount == 5; // Debug specific separations
 
+		double dot = mf.stepSeparationAxis.dotProduct(axis);
+		if (dot != 0 && discreteCollision) {
+			Vec3d cross = axis.crossProduct(mf.stepSeparationAxis);
+			double dotSeparation = signum(dot) * TL - (rA + rB);
+			double stepSeparation = -dotSeparation;
+			Vec3d stepSeparationVec = axis;
+
+			if (!cross.equals(Vec3d.ZERO)) {
+				Vec3d sepVec = normalizedAxis.scale(dotSeparation);
+				Vec3d axisPlane = axis.crossProduct(cross);
+				Vec3d stepPlane = mf.stepSeparationAxis.crossProduct(cross);
+				stepSeparationVec =
+					sepVec.subtract(axisPlane.scale(sepVec.dotProduct(stepPlane) / axisPlane.dotProduct(stepPlane)));
+				stepSeparation = stepSeparationVec.length();
+
+
+				if (abs(mf.stepSeparation) > abs(stepSeparation) && stepSeparation != 0) {
+//					CollisionDebugger.showDebugLine(Vec3d.ZERO, sepVec, 0x111155, "stepsep", -16);
+					mf.stepSeparation = stepSeparation;
+				}
+
+			} else {
+				if (abs(mf.stepSeparation) > stepSeparation) {
+					mf.stepSeparation = stepSeparation;
+//					CollisionDebugger.showDebugLine(Vec3d.ZERO, stepSeparationVec, 0xff9999, "axis", -16);
+				}
+			}
+
+//			if (abs(mf.separation) < abs(stepSeparation) && stepSeparation != 0)
+		}
+
 		if (isBestSeperation) {
 
-			mf.axis = axis.normalize();
-			mf.separation = value;
+			mf.axis = normalizedAxis;
+			mf.separation = seperation;
 
 			// Visualize values
 //			if (CollisionDebugger.AABB != null) {
@@ -113,6 +148,9 @@ public class ContinuousOBBCollider extends OBBCollider {
 		double earliestCollisionExitTime = Double.MAX_VALUE;
 		boolean isDiscreteCollision = true;
 
+		Vec3d stepSeparationAxis;
+		double stepSeparation;
+
 		public double getTimeOfImpact() {
 			if (latestCollisionEntryTime == UNDEFINED)
 				return UNDEFINED;
@@ -131,15 +169,22 @@ public class ContinuousOBBCollider extends OBBCollider {
 				.scale(getTimeOfImpact() * length);
 		}
 
-		@Override
-		public Vec3d asSeparationVec() {
-			if (isDiscreteCollision)
+		public Vec3d asSeparationVec(double obbStepHeight) {
+			if (isDiscreteCollision) {
+				if (stepSeparation <= obbStepHeight) 
+					return createSeparationVec(stepSeparation, stepSeparationAxis);
 				return super.asSeparationVec();
+			}
 			double t = getTimeOfImpact();
 			if (t == UNDEFINED)
 				return null;
 			return Vec3d.ZERO;
 		}
+		
+		@Override
+		public Vec3d asSeparationVec() {
+			return asSeparationVec(0);
+		}
 
 	}
 

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

@@ -95,6 +95,11 @@ public class OBBCollider {
 
 		public Vec3d asSeparationVec() {
 			double sep = separation;
+			Vec3d axis = this.axis;
+			return createSeparationVec(sep, axis);
+		}
+
+		protected Vec3d createSeparationVec(double sep, Vec3d axis) {
 			return axis.normalize()
 				.scale(signum(sep) * (abs(sep) + 1E-4));
 		}