S.A.T. will separate thee!

- Implemented a prototype of a new collision resolver that supports rotated bounding boxes

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

@@ -3,8 +3,14 @@ package com.simibubi.create.content.contraptions.components.structureMovement;
 import java.util.HashMap;
 import java.util.Map;
 
+import org.apache.commons.lang3.mutable.MutableBoolean;
+
 import com.simibubi.create.AllBlocks;
 import com.simibubi.create.content.contraptions.components.actors.BlockBreakingMovementBehaviour;
+import com.simibubi.create.foundation.collision.Matrix3d;
+import com.simibubi.create.foundation.collision.OrientedBB;
+import com.simibubi.create.foundation.utility.AngleHelper;
+import com.simibubi.create.foundation.utility.VecHelper;
 
 import net.minecraft.block.BlockState;
 import net.minecraft.block.CocoaBlock;
@@ -23,7 +29,6 @@ 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.ISelectionContext;
 import net.minecraft.util.math.shapes.VoxelShape;
 import net.minecraft.world.World;
 import net.minecraft.world.gen.feature.template.Template.BlockInfo;
@@ -43,10 +48,11 @@ public class ContraptionCollider {
 			return;
 
 		World world = contraptionEntity.getEntityWorld();
-		Vec3d contraptionMotion = contraptionEntity.getMotion();
+//		Vec3d contraptionMotion = contraptionEntity.getMotion();
 		Contraption contraption = contraptionEntity.getContraption();
 		AxisAlignedBB bounds = contraptionEntity.getBoundingBox();
 		Vec3d contraptionPosition = contraptionEntity.getPositionVec();
+		Vec3d contraptionRotation = contraptionEntity.getRotationVec();
 		contraptionEntity.collidingEntities.clear();
 
 		if (contraption == null)
@@ -55,41 +61,104 @@ public class ContraptionCollider {
 			return;
 
 		for (Entity entity : world.getEntitiesWithinAABB((EntityType<?>) null, bounds.grow(1),
-				e -> canBeCollidedWith(e))) {
+			e -> canBeCollidedWith(e))) {
-
-			ReuseableStream<VoxelShape> potentialHits =
-				getPotentiallyCollidedShapes(world, contraption, contraptionPosition, entity);
-			if (potentialHits.createStream().count() == 0)
-				continue;
-
-			Vec3d positionOffset = contraptionPosition.scale(-1);
-			AxisAlignedBB entityBB = entity.getBoundingBox().offset(positionOffset).grow(1.0E-7D);
-			Vec3d entityMotion = entity.getMotion();
-			Vec3d relativeMotion = entityMotion.subtract(contraptionMotion);
-			Vec3d allowedMovement = Entity.getAllowedMovement(relativeMotion, entityBB, world,
-					ISelectionContext.forEntity(entity), potentialHits);
-			potentialHits.createStream()
-					.forEach(voxelShape -> pushEntityOutOfShape(entity, voxelShape, positionOffset, contraptionMotion));
-
-			contraptionEntity.collidingEntities.add(entity);
-
-			if (allowedMovement.equals(relativeMotion))
-				continue;
-			
-			if (allowedMovement.y != relativeMotion.y) {
-				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;
 			if (entity instanceof PlayerEntity && !world.isRemote)
 				return;
 
-			entity.setMotion(allowedMovement.add(contraptionMotion));
+			Vec3d centerOf = VecHelper.getCenterOf(BlockPos.ZERO);
-			entity.velocityChanged = true;
+			Vec3d entityPosition = entity.getPositionVec();
+			Vec3d position = entityPosition.subtract(contraptionPosition)
+				.subtract(centerOf);
+			position =
+				VecHelper.rotate(position, -contraptionRotation.x, -contraptionRotation.y, -contraptionRotation.z);
+			position = position.add(centerOf)
+				.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;
+
+			MutableBoolean onCollide = new MutableBoolean(true);
+			potentialHits.createStream()
+				.forEach(shape -> {
+					AxisAlignedBB bb = shape.getBoundingBox();
+					Vec3d intersect = obb.intersect(bb);
+					if (intersect == null)
+						return;
+					intersect = VecHelper.rotate(intersect, contraptionRotation.x, contraptionRotation.y,
+						contraptionRotation.z);
+
+					obb.setCenter(obb.getCenter()
+						.add(intersect));
+					entity.move(MoverType.PISTON, intersect);
+
+					Vec3d entityMotion = entity.getMotion();
+					if (entityMotion.getX() > 0 == intersect.getX() < 0)
+						entityMotion = entityMotion.mul(0, 1, 1);
+					if (entityMotion.getY() > 0 == intersect.getY() < 0)
+						entityMotion = entityMotion.mul(1, 0, 1);
+					if (entityMotion.getZ() > 0 == intersect.getZ() < 0)
+						entityMotion = entityMotion.mul(1, 1, 0);
+					entity.setMotion(entityMotion);
+
+					if (onCollide.isTrue()) {
+						onCollide.setFalse();
+						contraptionEntity.collidingEntities.add(entity);
+						entity.velocityChanged = true;
+					}
+
+					if (intersect.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;
+				});
+
+//			Vec3d positionOffset = contraptionPosition.scale(-1);
+//			AxisAlignedBB entityBB = entity.getBoundingBox()
+//				.offset(positionOffset)
+//				.grow(1.0E-7D);
+//			Vec3d entityMotion = entity.getMotion();
+//			Vec3d relativeMotion = entityMotion.subtract(contraptionMotion);
+//			Vec3d allowedMovement = Entity.getAllowedMovement(relativeMotion, entityBB, world,
+//				ISelectionContext.forEntity(entity), potentialHits);
+//			potentialHits.createStream()
+//				.forEach(voxelShape -> pushEntityOutOfShape(entity, voxelShape, positionOffset, contraptionMotion));
+//
+//
+//			if (allowedMovement.equals(relativeMotion))
+//				continue;
+//
+//			if (allowedMovement.y != relativeMotion.y) {
+//				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;
+//			if (entity instanceof PlayerEntity && !world.isRemote)
+//				return;
+//
+//			entity.setMotion(allowedMovement.add(contraptionMotion));
 		}
 
 	}
@@ -112,11 +181,14 @@ public class ContraptionCollider {
 	}
 
 	public static void pushEntityOutOfShape(Entity entity, VoxelShape voxelShape, Vec3d positionOffset,
-			Vec3d shapeMotion) {
+		Vec3d shapeMotion) {
-		AxisAlignedBB entityBB = entity.getBoundingBox().offset(positionOffset);
+		AxisAlignedBB entityBB = entity.getBoundingBox()
+			.offset(positionOffset);
 		Vec3d entityMotion = entity.getMotion();
 
-		if (!voxelShape.toBoundingBoxList().stream().anyMatch(entityBB::intersects))
+		if (!voxelShape.toBoundingBoxList()
+			.stream()
+			.anyMatch(entityBB::intersects))
 			return;
 
 		AxisAlignedBB shapeBB = voxelShape.getBoundingBox();
@@ -127,8 +199,7 @@ public class ContraptionCollider {
 		for (Direction face : Direction.values()) {
 			Axis axis = face.getAxis();
 			double d = axis == Axis.X ? entityBB.getXSize() + shapeBB.getXSize()
-					: axis == Axis.Y ? entityBB.getYSize() + shapeBB.getYSize()
+				: axis == Axis.Y ? entityBB.getYSize() + shapeBB.getYSize() : entityBB.getZSize() + shapeBB.getZSize();
-							: entityBB.getZSize() + shapeBB.getZSize();
 			d = d + .5f;
 
 			Vec3d nudge = new Vec3d(face.getDirectionVec()).scale(d);
@@ -171,13 +242,14 @@ public class ContraptionCollider {
 	}
 
 	public static ReuseableStream<VoxelShape> getPotentiallyCollidedShapes(World world, Contraption contraption,
-			Vec3d contraptionPosition, Entity entity) {
+		AxisAlignedBB localBB) {
-		AxisAlignedBB blockScanBB = entity.getBoundingBox().offset(contraptionPosition.scale(-1)).grow(.5f);
+		AxisAlignedBB blockScanBB = localBB.grow(.5f);
 		BlockPos min = new BlockPos(blockScanBB.minX, blockScanBB.minY, blockScanBB.minZ);
 		BlockPos max = new BlockPos(blockScanBB.maxX, blockScanBB.maxY, blockScanBB.maxZ);
 
-		ReuseableStream<VoxelShape> potentialHits =
+		ReuseableStream<VoxelShape> potentialHits = new ReuseableStream<>(BlockPos.getAllInBox(min, max)
-			new ReuseableStream<>(BlockPos.getAllInBox(min, max).filter(contraption.blocks::containsKey).map(p -> {
+			.filter(contraption.blocks::containsKey)
+			.map(p -> {
 				BlockState blockState = contraption.blocks.get(p).state;
 				BlockPos pos = contraption.blocks.get(p).pos;
 				VoxelShape collisionShape = blockState.getCollisionShape(world, p);
@@ -217,7 +289,7 @@ public class ContraptionCollider {
 
 		// Other moving Contraptions
 		for (ContraptionEntity otherContraptionEntity : world.getEntitiesWithinAABB(ContraptionEntity.class,
-				bounds.grow(1), e -> !e.equals(contraptionEntity))) {
+			bounds.grow(1), e -> !e.equals(contraptionEntity))) {
 
 			if (!otherContraptionEntity.collisionEnabled())
 				continue;
@@ -232,11 +304,13 @@ public class ContraptionCollider {
 			if (otherBounds == null)
 				return false;
 
-			if (!bounds.offset(motion).intersects(otherBounds.offset(otherMotion)))
+			if (!bounds.offset(motion)
+				.intersects(otherBounds.offset(otherMotion)))
 				continue;
 
 			for (BlockPos colliderPos : contraption.getColliders(world, movementDirection)) {
-				colliderPos = colliderPos.add(gridPos).subtract(new BlockPos(otherPosition));
+				colliderPos = colliderPos.add(gridPos)
+					.subtract(new BlockPos(otherPosition));
 				if (!otherContraption.blocks.containsKey(colliderPos))
 					continue;
 				return true;
@@ -247,7 +321,7 @@ public class ContraptionCollider {
 	}
 
 	public static boolean isCollidingWithWorld(World world, Contraption contraption, BlockPos anchor,
-			Direction movementDirection) {
+		Direction movementDirection) {
 		for (BlockPos pos : contraption.getColliders(world, movementDirection)) {
 			BlockPos colliderPos = pos.add(anchor);
 
@@ -263,7 +337,8 @@ public class ContraptionCollider {
 					BlockBreakingMovementBehaviour behaviour =
 						(BlockBreakingMovementBehaviour) block.getMovementBehaviour();
 					if (!behaviour.canBreak(world, colliderPos, collidedState)
-							&& !collidedState.getCollisionShape(world, pos).isEmpty()) {
+						&& !collidedState.getCollisionShape(world, pos)
+							.isEmpty()) {
 						return true;
 					}
 					continue;
@@ -271,12 +346,14 @@ public class ContraptionCollider {
 			}
 
 			if (AllBlocks.PULLEY_MAGNET.has(collidedState) && pos.equals(BlockPos.ZERO)
-					&& movementDirection == Direction.UP)
+				&& movementDirection == Direction.UP)
 				continue;
 			if (collidedState.getBlock() instanceof CocoaBlock)
 				continue;
-			if (!collidedState.getMaterial().isReplaceable()
+			if (!collidedState.getMaterial()
-					&& !collidedState.getCollisionShape(world, colliderPos).isEmpty()) {
+				.isReplaceable()
+				&& !collidedState.getCollisionShape(world, colliderPos)
+					.isEmpty()) {
 				return true;
 			}
 

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

@@ -14,7 +14,6 @@ import com.simibubi.create.AllEntityTypes;
 import com.simibubi.create.content.contraptions.components.structureMovement.bearing.BearingContraption;
 import com.simibubi.create.content.contraptions.components.structureMovement.mounted.CartAssemblerTileEntity.CartMovementMode;
 import com.simibubi.create.content.contraptions.components.structureMovement.mounted.MountedContraption;
-import com.simibubi.create.content.contraptions.components.structureMovement.piston.LinearActuatorTileEntity;
 import com.simibubi.create.foundation.item.ItemHelper;
 import com.simibubi.create.foundation.networking.AllPackets;
 import com.simibubi.create.foundation.utility.AngleHelper;
@@ -136,7 +135,7 @@ public class ContraptionEntity extends Entity implements IEntityAdditionalSpawnD
 	}
 
 	public boolean collisionEnabled() {
-		return getController() instanceof LinearActuatorTileEntity;
+		return true;
 	}
 
 	@Override
@@ -624,5 +623,9 @@ public class ContraptionEntity extends Entity implements IEntityAdditionalSpawnD
 	public float getInitialAngle() {
 		return initialAngle;
 	}
+	
+	public Vec3d getRotationVec() {
+		return new Vec3d(pitch, yaw, roll);
+	}
 
 }

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

@@ -46,7 +46,7 @@ public class MechanicalBearingTileEntity extends GeneratingKineticTileEntity imp
 	public void addBehaviours(List<TileEntityBehaviour> behaviours) {
 		super.addBehaviours(behaviours);
 		movementMode = new ScrollOptionBehaviour<>(RotationMode.class, Lang.translate("contraptions.movement_mode"),
-				this, getMovementModeSlot());
+			this, getMovementModeSlot());
 		movementMode.requiresWrench();
 		behaviours.add(movementMode);
 	}
@@ -64,7 +64,7 @@ public class MechanicalBearingTileEntity extends GeneratingKineticTileEntity imp
 	public void neighbourChanged() {
 		if (!hasWorld())
 			return;
-		
+
 		boolean shouldWindmill = world.isBlockPowered(pos);
 		if (shouldWindmill == isWindmill)
 			return;
@@ -153,7 +153,8 @@ public class MechanicalBearingTileEntity extends GeneratingKineticTileEntity imp
 	}
 
 	public void assemble() {
-		if (!(world.getBlockState(pos).getBlock() instanceof MechanicalBearingBlock))
+		if (!(world.getBlockState(pos)
+			.getBlock() instanceof MechanicalBearingBlock))
 			return;
 
 		Direction direction = getBlockState().get(FACING);
@@ -168,7 +169,8 @@ public class MechanicalBearingTileEntity extends GeneratingKineticTileEntity imp
 			return;
 		contraption.removeBlocksFromWorld(world, BlockPos.ZERO);
 
-		movedContraption = ContraptionEntity.createStationary(world, contraption).controlledBy(this);
+		movedContraption = ContraptionEntity.createStationary(world, contraption)
+			.controlledBy(this);
 		BlockPos anchor = pos.offset(direction);
 		movedContraption.setPosition(anchor.getX(), anchor.getY(), anchor.getZ());
 		world.addEntity(movedContraption);
@@ -206,7 +208,8 @@ public class MechanicalBearingTileEntity extends GeneratingKineticTileEntity imp
 
 		if (world.isRemote)
 			clientAngleDiff /= 2;
-
+		if (movedContraption != null)
+			movedContraption.collisionTick();
 		if (running && Contraption.isFrozen())
 			disassemble();
 
@@ -214,11 +217,12 @@ public class MechanicalBearingTileEntity extends GeneratingKineticTileEntity imp
 			assembleNextTick = false;
 			if (running) {
 				boolean canDisassemble = movementMode.get() == RotationMode.ROTATE_PLACE
-						|| (isNearInitialAngle() && movementMode.get() == RotationMode.ROTATE_PLACE_RETURNED);
+					|| (isNearInitialAngle() && movementMode.get() == RotationMode.ROTATE_PLACE_RETURNED);
 				if (speed == 0 && (canDisassemble || movedContraption == null
-						|| movedContraption.getContraption().blocks.isEmpty())) {
+					|| movedContraption.getContraption().blocks.isEmpty())) {
 					if (movedContraption != null)
-						movedContraption.getContraption().stop(world);
+						movedContraption.getContraption()
+							.stop(world);
 					disassemble();
 				}
 				return;
@@ -255,9 +259,11 @@ public class MechanicalBearingTileEntity extends GeneratingKineticTileEntity imp
 
 	protected void applyRotation() {
 		if (movedContraption != null) {
-			Axis axis = getBlockState().get(FACING).getAxis();
+			Axis axis = getBlockState().get(FACING)
+				.getAxis();
 			Direction direction = Direction.getFacingFromAxis(AxisDirection.POSITIVE, axis);
-			Vec3d vec = new Vec3d(1, 1, 1).scale(angle).mul(new Vec3d(direction.getDirectionVec()));
+			Vec3d vec = new Vec3d(1, 1, 1).scale(angle)
+				.mul(new Vec3d(direction.getDirectionVec()));
 			movedContraption.rotateTo(vec.x, vec.y, vec.z);
 		}
 	}
@@ -294,14 +300,14 @@ public class MechanicalBearingTileEntity extends GeneratingKineticTileEntity imp
 	protected ValueBoxTransform getMovementModeSlot() {
 		return new DirectionalExtenderScrollOptionSlot((state, d) -> {
 			Axis axis = d.getAxis();
-			Axis bearingAxis = state.get(MechanicalBearingBlock.FACING).getAxis();
+			Axis bearingAxis = state.get(MechanicalBearingBlock.FACING)
+				.getAxis();
 			return bearingAxis != axis;
 		});
 	}
 
 	@Override
-	public void collided() {
+	public void collided() {}
-	}
 
 	@Override
 	public boolean isAttachedTo(ContraptionEntity contraption) {

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

@@ -0,0 +1,117 @@
+package com.simibubi.create.foundation.collision;
+
+import net.minecraft.util.math.MathHelper;
+import net.minecraft.util.math.Vec3d;
+
+public class Matrix3d {
+
+	double m00, m01, m02;
+	double m10, m11, m12;
+	double m20, m21, m22;
+
+	public Matrix3d asIdentity() {
+		m00 = m11 = m22 = 1;
+		m01 = m02 = m10 = m12 = m20 = m21 = 0;
+		return this;
+	}
+
+	public Matrix3d asXRotation(float radians) {
+		asIdentity();
+		double s = MathHelper.sin(radians);
+		double c = MathHelper.cos(radians);
+		m22 = m11 = c;
+		m21 = s;
+		m12 = -s;
+		return this;
+	}
+
+	public Matrix3d asYRotation(float radians) {
+		asIdentity();
+		double s = MathHelper.sin(radians);
+		double c = MathHelper.cos(radians);
+		m00 = m22 = c;
+		m20 = s;
+		m02 = -s;
+		return this;
+	}
+
+	public Matrix3d asZRotation(float radians) {
+		asIdentity();
+		double s = MathHelper.sin(radians);
+		double c = MathHelper.cos(radians);
+		m00 = m11 = c;
+		m01 = -s;
+		m10 = s;
+		return this;
+	}
+
+	public Matrix3d transpose() {
+		double d = m01;
+		m01 = m10;
+		m10 = d;
+		d = m02;
+		m02 = m20;
+		m20 = d;
+		d = m12;
+		m12 = m21;
+		m21 = d;
+		return this;
+	}
+
+	public Matrix3d scale(double d) {
+		m00 *= d;
+		m11 *= d;
+		m22 *= d;
+		return this;
+	}
+
+	public Matrix3d add(Matrix3d matrix) {
+		m00 += matrix.m00;
+		m01 += matrix.m01;
+		m02 += matrix.m02;
+		m10 += matrix.m10;
+		m11 += matrix.m11;
+		m12 += matrix.m12;
+		m20 += matrix.m20;
+		m21 += matrix.m21;
+		m22 += matrix.m22;
+		return this;
+	}
+
+	public Matrix3d multiply(Matrix3d m) {
+		double new00 = m00 * m.m00 + m01 * m.m10 + m02 * m.m20;
+		double new01 = m00 * m.m01 + m01 * m.m11 + m02 * m.m21;
+		double new02 = m00 * m.m02 + m01 * m.m12 + m02 * m.m22;
+		double new10 = m10 * m.m00 + m11 * m.m10 + m12 * m.m20;
+		double new11 = m10 * m.m01 + m11 * m.m11 + m12 * m.m21;
+		double new12 = m10 * m.m02 + m11 * m.m12 + m12 * m.m22;
+		double new20 = m20 * m.m00 + m21 * m.m10 + m22 * m.m20;
+		double new21 = m20 * m.m01 + m21 * m.m11 + m22 * m.m21;
+		double new22 = m20 * m.m02 + m21 * m.m12 + m22 * m.m22;
+		m00 = new00;
+		m01 = new01;
+		m02 = new02;
+		m10 = new10;
+		m11 = new11;
+		m12 = new12;
+		m20 = new20;
+		m21 = new21;
+		m22 = new22;
+		return this;
+	}
+
+	public Vec3d transform(Vec3d vec) {
+		double x = vec.x;
+		double y = vec.y;
+		double z = vec.z;
+		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);
+	}
+
+	public Matrix3d copy() {
+		return new Matrix3d().add(this);
+	}
+
+}

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

@@ -0,0 +1,148 @@
+package com.simibubi.create.foundation.collision;
+
+import static java.lang.Math.abs;
+
+import org.apache.commons.lang3.mutable.MutableDouble;
+import org.apache.commons.lang3.mutable.MutableObject;
+
+import net.minecraft.util.math.AxisAlignedBB;
+import net.minecraft.util.math.Vec3d;
+
+public class OrientedBB {
+
+	Vec3d center;
+	Vec3d extents;
+	Matrix3d rotation;
+
+	public OrientedBB(AxisAlignedBB bb) {
+		this(bb.getCenter(), extentsFromBB(bb), new Matrix3d().asIdentity());
+	}
+
+	public OrientedBB() {
+		this(Vec3d.ZERO, Vec3d.ZERO, new Matrix3d().asIdentity());
+	}
+
+	public OrientedBB(Vec3d center, Vec3d extents, Matrix3d rotation) {
+		this.setCenter(center);
+		this.extents = extents;
+		this.setRotation(rotation);
+	}
+
+	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.transpose());
+		// clean up
+		rotation.transpose();
+		return intersects;
+	}
+
+	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.m01, m.m02);
+		Vec3d uB1 = new Vec3d(m.m10, m.m11, m.m12);
+		Vec3d uB2 = new Vec3d(m.m20, m.m21, 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)
+
+			// 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;
+		if (distance != 0 && -diff < abs(bestSeparation.getValue())) {
+			bestAxis.setValue(axis);
+			bestSeparation.setValue(Math.signum(TL) * abs(diff));
+		}
+
+		return false;
+	}
+
+	public Matrix3d getRotation() {
+		return rotation;
+	}
+	
+	public void setRotation(Matrix3d rotation) {
+		this.rotation = rotation;
+	}
+
+	public Vec3d getCenter() {
+		return center;
+	}
+	
+	public void setCenter(Vec3d center) {
+		this.center = center;
+	}
+
+}