More collision polish

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

@@ -1,10 +1,13 @@
 package com.simibubi.create.content.contraptions.components.structureMovement;
 
-import java.util.ArrayList;
+import java.util.IdentityHashMap;
+import java.util.Iterator;
 import java.util.List;
+import java.util.Map;
 import java.util.Map.Entry;
 import java.util.UUID;
 
+import org.apache.commons.lang3.mutable.MutableInt;
 import org.apache.commons.lang3.tuple.MutablePair;
 
 import com.simibubi.create.AllMovementBehaviours;
@@ -49,7 +52,7 @@ public abstract class AbstractContraptionEntity extends Entity implements IEntit
 	private static final DataParameter<Boolean> STALLED =
 		EntityDataManager.createKey(AbstractContraptionEntity.class, DataSerializers.BOOLEAN);
 
-	public final List<Entity> collidingEntities = new ArrayList<>();
+	public final Map<Entity, MutableInt> collidingEntities;
 
 	protected Contraption contraption;
 	protected boolean initialized;
@@ -58,6 +61,7 @@ public abstract class AbstractContraptionEntity extends Entity implements IEntit
 	public AbstractContraptionEntity(EntityType<?> entityTypeIn, World worldIn) {
 		super(entityTypeIn, worldIn);
 		prevPosInvalid = true;
+		collidingEntities = new IdentityHashMap<>();
 	}
 
 	protected void setContraption(Contraption contraption) {
@@ -205,6 +209,13 @@ public abstract class AbstractContraptionEntity extends Entity implements IEntit
 			return;
 		}
 
+		for (Iterator<Entry<Entity, MutableInt>> iterator = collidingEntities.entrySet()
+			.iterator(); iterator.hasNext();)
+			if (iterator.next()
+				.getValue()
+				.incrementAndGet() > 3)
+				iterator.remove();
+
 		prevPosX = getX();
 		prevPosY = getY();
 		prevPosZ = getZ();
@@ -378,8 +389,13 @@ public abstract class AbstractContraptionEntity extends Entity implements IEntit
 			return;
 		if (contraption == null)
 			return;
+
 		remove();
+
 		StructureTransform transform = makeStructureTransform();
+		AllPackets.channel.send(PacketDistributor.TRACKING_ENTITY.with(() -> this),
+			new ContraptionDisassemblyPacket(this.getEntityId(), transform));
+
 		contraption.addBlocksToWorld(world, transform);
 		contraption.addPassengersToWorld(world, transform, getPassengers());
 
@@ -396,14 +412,17 @@ public abstract class AbstractContraptionEntity extends Entity implements IEntit
 		}
 
 		removePassengers();
+		moveCollidedEntitiesOnDisassembly(transform);
+	}
 
-		for (Entity entity : collidingEntities) {
+	private void moveCollidedEntitiesOnDisassembly(StructureTransform transform) {
-			Vec3d positionVec = getPositionVec();
+		for (Entity entity : collidingEntities.keySet()) {
-			Vec3d localVec = entity.getPositionVec()
+			Vec3d localVec = toLocalVector(entity.getPositionVec(), 0);
-				.subtract(positionVec);
-			localVec = reverseRotation(localVec, 1);
 			Vec3d transformed = transform.apply(localVec);
-			entity.setPositionAndUpdate(transformed.x, transformed.y, transformed.z);
+			if (world.isRemote)
+				entity.setPosition(transformed.x, transformed.y + 1 / 16f, transformed.z);
+			else
+				entity.setPositionAndUpdate(transformed.x, transformed.y + 1 / 16f, transformed.z);
 		}
 	}
 
@@ -449,6 +468,15 @@ public abstract class AbstractContraptionEntity extends Entity implements IEntit
 		ce.handleStallInformation(packet.x, packet.y, packet.z, packet.angle);
 	}
 
+	@OnlyIn(Dist.CLIENT)
+	static void handleDisassemblyPacket(ContraptionDisassemblyPacket packet) {
+  		Entity entity = Minecraft.getInstance().world.getEntityByID(packet.entityID);
+		if (!(entity instanceof AbstractContraptionEntity))
+			return;
+		AbstractContraptionEntity ce = (AbstractContraptionEntity) entity;
+		ce.moveCollidedEntitiesOnDisassembly(packet.transform);
+	}
+
 	protected abstract float getStalledAngle();
 
 	protected abstract void handleStallInformation(float x, float y, float z, float angle);

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

@@ -8,6 +8,8 @@ import java.util.List;
 import java.util.stream.Stream;
 
 import org.apache.commons.lang3.mutable.MutableBoolean;
+import org.apache.commons.lang3.mutable.MutableFloat;
+import org.apache.commons.lang3.mutable.MutableInt;
 import org.apache.commons.lang3.mutable.MutableObject;
 
 import com.google.common.base.Predicates;
@@ -65,8 +67,6 @@ public class ContraptionCollider {
 		if (bounds == null)
 			return;
 
-		contraptionEntity.collidingEntities.clear();
-
 		Vec3d contraptionPosition = contraptionEntity.getPositionVec();
 		Vec3d contraptionMotion = contraptionPosition.subtract(contraptionEntity.getPrevPositionVec());
 		Vec3d anchorVec = contraptionEntity.getAnchorVec();
@@ -123,13 +123,12 @@ public class ContraptionCollider {
 			// Prepare entity bounds
 			OrientedBB obb = new OrientedBB(localBB);
 			obb.setRotation(rotationMatrix);
-			motion = rotationMatrix.transform(motion);
 			motion = motion.subtract(contraptionMotion);
+			motion = rotationMatrix.transform(motion);
 
 			MutableObject<Vec3d> collisionResponse = new MutableObject<>(Vec3d.ZERO);
-			MutableObject<Vec3d> allowedMotion = new MutableObject<>(motion);
-			MutableBoolean futureCollision = new MutableBoolean(false);
 			MutableBoolean surfaceCollision = new MutableBoolean(false);
+			MutableFloat temporalResponse = new MutableFloat(1);
 			Vec3d obbCenter = obb.getCenter();
 
 			// Apply separation maths
@@ -140,21 +139,22 @@ public class ContraptionCollider {
 
 			boolean doHorizontalPass = !rotation.hasVerticalRotation();
 			for (boolean horizontalPass : Iterate.trueAndFalse) {
+				boolean verticalPass = !horizontalPass || !doHorizontalPass;
 
 				for (AxisAlignedBB bb : bbs) {
 					Vec3d currentResponse = collisionResponse.getValue();
 					obb.setCenter(obbCenter.add(currentResponse));
-					ContinuousSeparationManifold intersect = obb.intersect(bb, allowedMotion.getValue());
+					ContinuousSeparationManifold intersect = obb.intersect(bb, motion);
 
 					if (intersect == null)
 						continue;
-					if ((!horizontalPass || !doHorizontalPass) && surfaceCollision.isFalse())
+					if (verticalPass && surfaceCollision.isFalse())
 						surfaceCollision.setValue(intersect.isSurfaceCollision());
 
 					double timeOfImpact = intersect.getTimeOfImpact();
 					if (timeOfImpact > 0 && timeOfImpact < 1) {
-						futureCollision.setTrue();
+						if (temporalResponse.getValue() > timeOfImpact)
-						allowedMotion.setValue(intersect.getAllowedMotion(allowedMotion.getValue()));
+							temporalResponse.setValue(timeOfImpact);
 						continue;
 					}
 
@@ -163,28 +163,28 @@ public class ContraptionCollider {
 						collisionResponse.setValue(currentResponse.add(separation));
 				}
 
-				if (!horizontalPass || !doHorizontalPass)
+				if (verticalPass)
 					break;
 
-				boolean noVerticalMotionResponse = allowedMotion.getValue().y == motion.y;
+				boolean noVerticalMotionResponse = temporalResponse.getValue() == 1;
 				boolean noVerticalCollision = collisionResponse.getValue().y == 0;
 				if (noVerticalCollision && noVerticalMotionResponse)
 					break;
 
 				// Re-run collisions with horizontal offset
 				collisionResponse.setValue(collisionResponse.getValue()
-					.mul(1, 0, 1));
+					.mul(129 / 128f, 0, 129 / 128f));
-				allowedMotion.setValue(allowedMotion.getValue()
-					.mul(1, 0, 1)
-					.add(0, motion.y, 0));
 				continue;
 			}
 
 			// Resolve collision
 			Vec3d entityMotion = entity.getMotion();
 			Vec3d totalResponse = collisionResponse.getValue();
-			Vec3d motionResponse = allowedMotion.getValue();
 			boolean hardCollision = !totalResponse.equals(Vec3d.ZERO);
+			boolean temporalCollision = temporalResponse.getValue() != 1;
+			Vec3d motionResponse = !temporalCollision ? motion
+				: motion.normalize()
+					.scale(motion.length() * temporalResponse.getValue());
 
 			rotationMatrix.transpose();
 			motionResponse = rotationMatrix.transform(motionResponse)
@@ -193,10 +193,11 @@ public class ContraptionCollider {
 			totalResponse = VecHelper.rotate(totalResponse, yawOffset, Axis.Y);
 			rotationMatrix.transpose();
 
-			if (futureCollision.isTrue() && playerType != PlayerType.SERVER) {
+			if (temporalCollision && playerType != PlayerType.SERVER) {
-				if (motionResponse.y != entityMotion.y) {
+				double idealVerticalMotion = motionResponse.y;
+				if (idealVerticalMotion != entityMotion.y) {
 					entity.setMotion(entityMotion.mul(1, 0, 1)
-						.add(0, motionResponse.y, 0));
+						.add(0, idealVerticalMotion, 0));
 					entityMotion = entity.getMotion();
 				}
 			}
@@ -213,7 +214,8 @@ public class ContraptionCollider {
 				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)
-					entityMotion = entityMotion.mul(1, 0, 1);
+					entityMotion = entityMotion.mul(1, 0, 1)
+						.add(0, contraptionMotion.y, 0);
 				if (motionZ != 0 && Math.abs(intersectZ) > horizonalEpsilon && motionZ > 0 == intersectZ < 0)
 					entityMotion = entityMotion.mul(1, 1, 0);
 			}
@@ -226,27 +228,25 @@ public class ContraptionCollider {
 				continue;
 			}
 
-//			totalResponse = totalResponse.add(contactPointMotion);
 			Vec3d allowedMovement = getAllowedMovement(totalResponse, entity);
-			contraptionEntity.collidingEntities.add(entity);
-			entity.velocityChanged = true;
 			entity.setPosition(entityPosition.x + allowedMovement.x, entityPosition.y + allowedMovement.y,
 				entityPosition.z + allowedMovement.z);
 			entityPosition = entity.getPositionVec();
 
+			entity.velocityChanged = true;
 			Vec3d contactPointMotion = Vec3d.ZERO;
+
 			if (surfaceCollision.isTrue()) {
 				entity.fallDistance = 0;
 				entity.onGround = true;
-				contraptionEntity.collidingEntities.add(entity);
+				contraptionEntity.collidingEntities.put(entity, new MutableInt(0));
 				if (entity instanceof ItemEntity)
 					entityMotion = entityMotion.mul(.5f, 1, .5f);
-
 				if (playerType != PlayerType.SERVER) {
 					contactPointMotion = contraptionEntity.getContactPointMotion(entityPosition);
 					allowedMovement = getAllowedMovement(contactPointMotion, entity);
-					entity.setPosition(entityPosition.x + allowedMovement.x, entityPosition.y + allowedMovement.y,
+					entity.setPosition(entityPosition.x + allowedMovement.x,
-						entityPosition.z + allowedMovement.z);
+						entityPosition.y, entityPosition.z + allowedMovement.z);
 				}
 			}
 
@@ -260,7 +260,8 @@ public class ContraptionCollider {
 			float limbSwing = MathHelper.sqrt(d0 * d0 + d1 * d1) * 4.0F;
 			if (limbSwing > 1.0F)
 				limbSwing = 1.0F;
-			AllPackets.channel.sendToServer(new ClientMotionPacket(entityMotion, true, limbSwing));
+			AllPackets.channel
+				.sendToServer(new ClientMotionPacket(entityMotion, true, limbSwing));
 		}
 
 	}

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

@@ -0,0 +1,42 @@
+package com.simibubi.create.content.contraptions.components.structureMovement;
+
+import java.util.function.Supplier;
+
+import com.simibubi.create.foundation.networking.SimplePacketBase;
+
+import net.minecraft.network.PacketBuffer;
+import net.minecraftforge.api.distmarker.Dist;
+import net.minecraftforge.fml.DistExecutor;
+import net.minecraftforge.fml.network.NetworkEvent.Context;
+
+public class ContraptionDisassemblyPacket extends SimplePacketBase {
+
+	int entityID;
+	StructureTransform transform;
+
+	public ContraptionDisassemblyPacket(int entityID, StructureTransform transform) {
+		this.entityID = entityID;
+		this.transform = transform;
+	}
+
+	public ContraptionDisassemblyPacket(PacketBuffer buffer) {
+		entityID = buffer.readInt();
+		transform = StructureTransform.fromBuffer(buffer);
+	}
+
+	@Override
+	public void write(PacketBuffer buffer) {
+		buffer.writeInt(entityID);
+		transform.writeToBuffer(buffer);
+	}
+
+	@Override
+	public void handle(Supplier<Context> context) {
+		context.get()
+			.enqueueWork(() -> DistExecutor.runWhenOn(Dist.CLIENT,
+				() -> () -> AbstractContraptionEntity.handleDisassemblyPacket(this)));
+		context.get()
+			.setPacketHandled(true);
+	}
+
+}

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

@@ -50,6 +50,13 @@ public class ControlledContraptionEntity extends AbstractContraptionEntity {
 		return contraption instanceof TranslatingContraption;
 	}
 	
+	@Override
+		public Vec3d getContactPointMotion(Vec3d globalContactPoint) {
+			if (contraption instanceof TranslatingContraption)
+				return getMotion();
+			return super.getContactPointMotion(globalContactPoint);
+		}
+
 	@Override
 	protected void setContraption(Contraption contraption) {
 		super.setContraption(contraption);

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

@@ -19,6 +19,7 @@ import net.minecraft.block.BlockState;
 import net.minecraft.block.HorizontalFaceBlock;
 import net.minecraft.block.SlabBlock;
 import net.minecraft.block.StairsBlock;
+import net.minecraft.network.PacketBuffer;
 import net.minecraft.state.BooleanProperty;
 import net.minecraft.state.properties.AttachFace;
 import net.minecraft.state.properties.BellAttachment;
@@ -40,6 +41,13 @@ public class StructureTransform {
 	Axis rotationAxis;
 	BlockPos offset;
 
+	private StructureTransform(BlockPos offset, int angle, Axis axis, Rotation rotation) {
+		this.offset = offset;
+		this.angle = angle;
+		rotationAxis = axis;
+		this.rotation = rotation;
+	}
+
 	public StructureTransform(BlockPos offset, float xRotation, float yRotation, float zRotation) {
 		this.offset = offset;
 		if (xRotation != 0) {
@@ -71,6 +79,7 @@ public class StructureTransform {
 
 	public Vec3d apply(Vec3d localVec) {
 		Vec3d vec = localVec;
+		if (rotationAxis != null)
 			vec = VecHelper.rotateCentered(vec, angle, rotationAxis);
 		vec = vec.add(new Vec3d(offset));
 		return vec;
@@ -78,6 +87,7 @@ public class StructureTransform {
 
 	public BlockPos apply(BlockPos localPos) {
 		Vec3d vec = VecHelper.getCenterOf(localPos);
+		if (rotationAxis != null)
 			vec = VecHelper.rotateCentered(vec, angle, rotationAxis);
 		localPos = new BlockPos(vec);
 		return localPos.add(offset);
@@ -202,7 +212,8 @@ public class StructureTransform {
 
 	protected BlockState transformBelt(BlockState state, boolean halfTurn) {
 		Direction initialDirection = state.get(BeltBlock.HORIZONTAL_FACING);
-		boolean diagonal = state.get(BeltBlock.SLOPE) == BeltSlope.DOWNWARD || state.get(BeltBlock.SLOPE) == BeltSlope.UPWARD;
+		boolean diagonal =
+			state.get(BeltBlock.SLOPE) == BeltSlope.DOWNWARD || state.get(BeltBlock.SLOPE) == BeltSlope.UPWARD;
 
 		if (!diagonal) {
 			for (int i = 0; i < rotation.ordinal(); i++) {
@@ -254,8 +265,7 @@ public class StructureTransform {
 				boolean downward = slope == BeltSlope.DOWNWARD;
 
 				// Rotate diagonal
-				if (direction.getAxisDirection() == AxisDirection.POSITIVE ^ downward
+				if (direction.getAxisDirection() == AxisDirection.POSITIVE ^ downward ^ direction.getAxis() == Axis.Z) {
-					^ direction.getAxis() == Axis.Z) {
 					state = state.with(BeltBlock.SLOPE, upward ? BeltSlope.DOWNWARD : BeltSlope.UPWARD);
 				} else {
 					state = state.with(BeltBlock.HORIZONTAL_FACING, newDirection);
@@ -269,8 +279,8 @@ public class StructureTransform {
 			boolean vertical = slope == BeltSlope.VERTICAL;
 
 			if (diagonal) {
-				state = state.with(BeltBlock.SLOPE,
+				state = state.with(BeltBlock.SLOPE, slope == BeltSlope.UPWARD ? BeltSlope.DOWNWARD
-					slope == BeltSlope.UPWARD ? BeltSlope.DOWNWARD : slope == BeltSlope.DOWNWARD ? BeltSlope.UPWARD : slope);
+					: slope == BeltSlope.DOWNWARD ? BeltSlope.UPWARD : slope);
 			} else if (vertical) {
 				state = state.with(BeltBlock.HORIZONTAL_FACING, newDirection);
 			}
@@ -317,4 +327,21 @@ public class StructureTransform {
 		return rotated;
 	}
 
+	public static StructureTransform fromBuffer(PacketBuffer buffer) {
+		BlockPos readBlockPos = buffer.readBlockPos();
+		int readAngle = buffer.readInt();
+		int axisIndex = buffer.readVarInt();
+		int rotationIndex = buffer.readVarInt();
+		return new StructureTransform(readBlockPos, readAngle,
+			axisIndex == -1 ? null : Axis.values()[axisIndex],
+			rotationIndex == -1 ? null : Rotation.values()[rotationIndex]);
+	}
+
+	public void writeToBuffer(PacketBuffer buffer) {
+		buffer.writeBlockPos(offset);
+		buffer.writeInt(angle);
+		buffer.writeVarInt(rotationAxis == null ? -1 : rotationAxis.ordinal());
+		buffer.writeVarInt(rotation == null ? -1 : rotation.ordinal());
+	}
+
 }

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

@@ -163,12 +163,6 @@ public class ContinuousOBBCollider extends OBBCollider {
 			return true;
 		}
 
-		public Vec3d getAllowedMotion(Vec3d motion) {
-			double length = motion.length();
-			return motion.normalize()
-				.scale(getTimeOfImpact() * length);
-		}
-
 		public Vec3d asSeparationVec(double obbStepHeight) {
 			if (isDiscreteCollision) {
 				if (stepSeparation <= obbStepHeight) 

src/main/java/com/simibubi/create/foundation/networking/AllPackets.java

@@ -5,6 +5,7 @@ import java.util.function.Function;
 import java.util.function.Supplier;
 
 import com.simibubi.create.Create;
+import com.simibubi.create.content.contraptions.components.structureMovement.ContraptionDisassemblyPacket;
 import com.simibubi.create.content.contraptions.components.structureMovement.ContraptionStallPacket;
 import com.simibubi.create.content.contraptions.components.structureMovement.glue.GlueEffectPacket;
 import com.simibubi.create.content.contraptions.components.structureMovement.sync.ClientMotionPacket;
@@ -25,8 +26,8 @@ import com.simibubi.create.content.logistics.packet.ConfigureStockswitchPacket;
 import com.simibubi.create.content.schematics.packet.ConfigureSchematicannonPacket;
 import com.simibubi.create.content.schematics.packet.InstantSchematicPacket;
 import com.simibubi.create.content.schematics.packet.SchematicPlacePacket;
-import com.simibubi.create.content.schematics.packet.SchematicUploadPacket;
 import com.simibubi.create.content.schematics.packet.SchematicSyncPacket;
+import com.simibubi.create.content.schematics.packet.SchematicUploadPacket;
 import com.simibubi.create.foundation.command.ConfigureConfigPacket;
 import com.simibubi.create.foundation.tileEntity.behaviour.filtering.FilteringCountUpdatePacket;
 import com.simibubi.create.foundation.tileEntity.behaviour.scrollvalue.ScrollValueUpdatePacket;
@@ -69,6 +70,7 @@ public enum AllPackets {
 	BEAM_EFFECT(ZapperBeamPacket.class, ZapperBeamPacket::new),
 	CONFIGURE_CONFIG(ConfigureConfigPacket.class, ConfigureConfigPacket::new),
 	CONTRAPTION_STALL(ContraptionStallPacket.class, ContraptionStallPacket::new),
+	CONTRAPTION_DISASSEMBLE(ContraptionDisassemblyPacket.class, ContraptionDisassemblyPacket::new),
 	GLUE_EFFECT(GlueEffectPacket.class, GlueEffectPacket::new),
 	CONTRAPTION_SEAT_MAPPING(ContraptionSeatMappingPacket.class, ContraptionSeatMappingPacket::new),
 	LIMBSWING_UPDATE(LimbSwingUpdatePacket.class, LimbSwingUpdatePacket::new),