Collision refinements

- Fixed reversed matrices and swapped rotation axes passed into the collision resolver
- Temporarily reduced collision manifold generation to collisions that are NOT edge-to-edge collisions.
- Enabled collision response for clockwork bearings

src/main/java/com/simibubi/create/ClientEvents.java

@@ -73,6 +73,7 @@ public class ClientEvents {
 		SuperRenderTypeBuffer buffer = SuperRenderTypeBuffer.getInstance();
 		CreateClient.schematicHandler.render(ms, buffer);
 		CreateClient.outliner.renderOutlines(ms, buffer);
+//		CollisionDebugger.render(ms, buffer);
 		buffer.draw();
 		
 		ms.pop();
@@ -110,6 +111,7 @@ public class ClientEvents {
 
 		double delta = event.getScrollDelta();
 
+//		CollisionDebugger.onScroll(delta);
 		boolean cancelled = CreateClient.schematicHandler.mouseScrolled(delta)
 			|| CreateClient.schematicAndQuillHandler.mouseScrolled(delta) || FilteringHandler.onScroll(delta)
 			|| ScrollValueHandler.onScroll(delta);

src/main/java/com/simibubi/create/CreateClient.java

@@ -109,6 +109,7 @@ public class CreateClient {
 		KineticDebugger.tick();
 		ZapperRenderHandler.tick();
 		ExtendoGripRenderHandler.tick();
+//		CollisionDebugger.tick();
 		outliner.tickOutlines();
 	}
 

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

@@ -65,32 +65,36 @@ public class ContraptionCollider {
 			if (entity instanceof PlayerEntity && !world.isRemote)
 				return;
 
-			Vec3d centerOf = VecHelper.getCenterOf(BlockPos.ZERO);
+			Vec3d centerOfBlock = VecHelper.getCenterOf(BlockPos.ZERO);
 			Vec3d entityPosition = entity.getPositionVec();
+			Vec3d centerY = new Vec3d(0, entity.getBoundingBox()
+				.getYSize() / 2, 0);
 			Vec3d position = entityPosition.subtract(contraptionPosition)
-				.subtract(centerOf);
+				.subtract(centerOfBlock)
+				.add(centerY);
 			position =
-				VecHelper.rotate(position, -contraptionRotation.x, -contraptionRotation.y, -contraptionRotation.z);
+				VecHelper.rotate(position, -contraptionRotation.z, -contraptionRotation.y, -contraptionRotation.x);
-			position = position.add(centerOf)
+			position = position.add(centerOfBlock)
+				.subtract(centerY)
 				.subtract(entityPosition);
 			AxisAlignedBB localBB = entity.getBoundingBox()
 				.offset(position)
 				.grow(1.0E-7D);
 
-			OrientedBB obb = new OrientedBB(localBB);
-			if (!contraptionRotation.equals(Vec3d.ZERO)) {
-				Matrix3d rotation = new Matrix3d().asIdentity();
-				rotation.multiply(new Matrix3d().asXRotation(AngleHelper.rad(contraptionRotation.x)));
-				rotation.multiply(new Matrix3d().asYRotation(AngleHelper.rad(contraptionRotation.y)));
-				rotation.multiply(new Matrix3d().asZRotation(AngleHelper.rad(contraptionRotation.z)));
-				obb.setRotation(rotation);
-			}
-
 			ReuseableStream<VoxelShape> potentialHits = getPotentiallyCollidedShapes(world, contraption, localBB);
 			if (potentialHits.createStream()
 				.count() == 0)
 				continue;
 
+			OrientedBB obb = new OrientedBB(localBB);
+			if (!contraptionRotation.equals(Vec3d.ZERO)) {
+				Matrix3d rotation = new Matrix3d().asIdentity();
+				rotation.multiply(new Matrix3d().asXRotation(AngleHelper.rad(contraptionRotation.z)));
+				rotation.multiply(new Matrix3d().asYRotation(AngleHelper.rad(contraptionRotation.y)));
+				rotation.multiply(new Matrix3d().asZRotation(AngleHelper.rad(contraptionRotation.x)));
+				obb.setRotation(rotation);
+			}
+
 			MutableBoolean onCollide = new MutableBoolean(true);
 			potentialHits.createStream()
 				.forEach(shape -> {
@@ -98,12 +102,12 @@ public class ContraptionCollider {
 					Vec3d intersect = obb.intersect(bb);
 					if (intersect == null)
 						return;
-					intersect = VecHelper.rotate(intersect, contraptionRotation.x, contraptionRotation.y,
+					intersect = VecHelper.rotate(intersect, contraptionRotation.z, contraptionRotation.y,
-						contraptionRotation.z);
+						contraptionRotation.x);
 
 					obb.setCenter(obb.getCenter()
 						.add(intersect));
-					entity.move(MoverType.PISTON, intersect);
+					entity.move(MoverType.PLAYER, intersect);
 
 					Vec3d entityMotion = entity.getMotion();
 					if (entityMotion.getX() > 0 == intersect.getX() < 0)

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

@@ -47,6 +47,10 @@ public class ClockworkBearingTileEntity extends KineticTileEntity implements IBe
 
 		if (running && Contraption.isFrozen())
 			disassemble();
+		if (hourHand != null)
+			hourHand.collisionTick();
+		if (minuteHand != null)
+			minuteHand.collisionTick();
 
 		if (!world.isRemote && assembleNextTick) {
 			assembleNextTick = false;

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

@@ -0,0 +1,82 @@
+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.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 {
+
+	static AxisAlignedBB staticBB = new AxisAlignedBB(BlockPos.ZERO.up(10));
+	static OrientedBB movingBB = new OrientedBB(new AxisAlignedBB(BlockPos.ZERO));
+	static Vec3d seperation;
+	static double angle = 0;
+	static AABBOutline outline;
+
+	public static void onScroll(double delta) {
+		angle += delta;
+		movingBB.setRotation(new Matrix3d().asZRotation(AngleHelper.rad(angle)));
+	}
+
+	public static void render(MatrixStack ms, SuperRenderTypeBuffer buffer) {
+		ms.push();
+		outline = new AABBOutline(movingBB.getAsAxisAlignedBB());
+		outline.getParams()
+			.withFaceTexture(seperation == null ? AllSpecialTextures.CHECKERED : null)
+			.colored(0xffffff);
+		if (seperation != null)
+			outline.getParams()
+				.lineWidth(1 / 64f)
+				.colored(0xff6544);
+		MatrixStacker.of(ms)
+			.translate(movingBB.center);
+		ms.peek()
+			.getModel()
+			.multiply(movingBB.rotation.getAsMatrix4f());
+		MatrixStacker.of(ms)
+			.translateBack(movingBB.center);
+		outline.render(ms, buffer);
+		ms.pop();
+
+		ms.push();
+		if (seperation != null) {
+			outline.getParams()
+				.colored(0x65ff44)
+				.lineWidth(1 / 32f);
+			MatrixStacker.of(ms)
+				.translate(seperation)
+				.translate(movingBB.center);
+			ms.peek()
+				.getModel()
+				.multiply(movingBB.rotation.getAsMatrix4f());
+			MatrixStacker.of(ms)
+				.translateBack(movingBB.center);
+			outline.render(ms, buffer);
+		}
+		ms.pop();
+	}
+
+	public static void tick() {
+		staticBB = new AxisAlignedBB(BlockPos.ZERO.up(60));
+		RayTraceResult mouse = Minecraft.getInstance().objectMouseOver;
+		if (mouse != null && mouse.getType() == Type.BLOCK) {
+			BlockRayTraceResult hit = (BlockRayTraceResult) mouse;
+			movingBB.setCenter(hit.getHitVec());
+			seperation = movingBB.intersect(staticBB);
+		}
+		CreateClient.outliner.showAABB(staticBB, staticBB)
+			.withFaceTexture(seperation == null ? AllSpecialTextures.CHECKERED : null);
+	}
+
+}

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

@@ -1,7 +1,10 @@
 package com.simibubi.create.foundation.collision;
 
+import net.minecraft.client.renderer.Matrix4f;
 import net.minecraft.util.math.MathHelper;
 import net.minecraft.util.math.Vec3d;
+import net.minecraftforge.api.distmarker.Dist;
+import net.minecraftforge.api.distmarker.OnlyIn;
 
 public class Matrix3d {
 
@@ -114,4 +117,25 @@ public class Matrix3d {
 		return new Matrix3d().add(this);
 	}
 	
+	float[] conversionBuffer = new float[16];
+	
+	@OnlyIn(Dist.CLIENT)
+	public Matrix4f getAsMatrix4f() {
+		for (int i = 0; i < 4; i++)
+			for (int j = 0; j < 4; j++)
+				conversionBuffer[j * 4 + i] = i == j ? 1 : 0;
+		
+		conversionBuffer[0] = (float) m00;
+		conversionBuffer[1] = (float) m01;
+		conversionBuffer[2] = (float) m02;
+		conversionBuffer[4] = (float) m10;
+		conversionBuffer[5] = (float) m11;
+		conversionBuffer[6] = (float) m12;
+		conversionBuffer[8] = (float) m20;
+		conversionBuffer[9] = (float) m21;
+		conversionBuffer[10] = (float) m22;
+		
+		return new Matrix4f(conversionBuffer);
+	}
+
 }

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

@@ -5,6 +5,8 @@ import static java.lang.Math.abs;
 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;
 
@@ -30,10 +32,7 @@ public class OrientedBB {
 
 	public Vec3d intersect(AxisAlignedBB bb) {
 		Vec3d extentsA = extentsFromBB(bb);
-		// Inverse rotation, to bring our OBB to AA space
+		Vec3d intersects = separateBBs(bb.getCenter(), center, extentsA, extents, rotation);
-		Vec3d intersects = separateBBs(bb.getCenter(), center, extentsA, extents, rotation.transpose());
-		// clean up
-		rotation.transpose();
 		return intersects;
 	}
 
@@ -60,9 +59,9 @@ public class OrientedBB {
 		Vec3d uA1 = new Vec3d(0, 1, 0);
 		Vec3d uA2 = new Vec3d(0, 0, 1);
 
-		Vec3d uB0 = new Vec3d(m.m00, m.m01, m.m02);
+		Vec3d uB0 = new Vec3d(m.m00, m.m10, m.m20);
-		Vec3d uB1 = new Vec3d(m.m10, m.m11, m.m12);
+		Vec3d uB1 = new Vec3d(m.m01, m.m11, m.m21);
-		Vec3d uB2 = new Vec3d(m.m20, m.m21, m.m22);
+		Vec3d uB2 = new Vec3d(m.m02, m.m12, m.m22);
 
 		checkCount = 0;
 
@@ -74,34 +73,42 @@ public class OrientedBB {
 			|| 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,
+			|| 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,
+			|| 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,
+			|| 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,
+//			|| 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)
+//				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,
+//			|| 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)
+//				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,
+//			|| 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)
+//				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,
+//			|| 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)
+//				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,
+//			|| 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)
+//				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,
+//			|| 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)
+//				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,
+//			|| 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)
+//				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,
+//			|| 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)
+//				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,
+//			|| 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)))
+//				eA.x * a12 + eA.y * a02, eB.x * a21 + eB.y * a20)
+		))
 
 			return bestAxis.getValue()
 				.normalize()
@@ -121,14 +128,37 @@ public class OrientedBB {
 		double diff = distance - (rA + rB);
 		if (diff > 0)
 			return true;
-		if (distance != 0 && -diff < abs(bestSeparation.getValue())) {
+		boolean isBestSeperation = distance != 0 && -(diff) <= abs(bestSeparation.getValue());
-			bestAxis.setValue(axis);
+//		boolean isBestSeperation = checkCount == 12; // Debug specific separations
-			bestSeparation.setValue(Math.signum(TL) * abs(diff));
+		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.staticBB.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;
 	}
@@ -145,4 +175,9 @@ public class OrientedBB {
 		this.center = center;
 	}
 
+	public AxisAlignedBB getAsAxisAlignedBB() {
+		return new AxisAlignedBB(0, 0, 0, 0, 0, 0).offset(center)
+			.grow(extents.x, extents.y, extents.z);
+	}
+
 }