diff --git a/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/ContraptionCollider.java b/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/ContraptionCollider.java index e91c2221d..fb1f6cac2 100644 --- a/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/ContraptionCollider.java +++ b/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>> 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), - contraptionEntity::canCollideWith)) { - if (entity instanceof PlayerEntity && !world.isRemote) - return; + for (Entity entity : world.getEntitiesWithinAABB((EntityType) null, bounds.grow(2) + .expand(0, 32, 0), contraptionEntity::canCollideWith)) { + boolean serverPlayer = entity instanceof PlayerEntity && !world.isRemote; + // 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() - .getYSize() / 2, 0); + AxisAlignedBB entityBounds = entity.getBoundingBox(); + 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); - - String nbtMotionKey = "ContraptionCollisionFeedback"; - 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 potentialHits = getPotentiallyCollidedShapes(world, contraption, localBB); + ReuseableStream potentialHits = + getPotentiallyCollidedShapes(world, contraption, localBB.expand(motion)); 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 collisionResponse = new MutableObject<>(Vec3d.ZERO); + MutableObject allowedMotion = new MutableObject<>(motion); + MutableBoolean futureCollision = new MutableBoolean(false); + MutableBoolean surfaceCollision = new MutableBoolean(false); Vec3d obbCenter = obb.getCenter(); + // Apply separation maths + List bbs = new ArrayList<>(); potentialHits.createStream() - .forEach(shape -> { - Vec3d currentResponse = collisionResponse.getValue(); - shape.toBoundingBoxList() - .parallelStream() - .forEach(bb -> { - obb.setCenter(obbCenter.add(currentResponse)); - Vec3d intersect = obb.intersect(bb); - if (intersect != null) - collisionResponse.setValue(currentResponse.add(intersect)); - }); - }); + .forEach(shape -> shape.toBoundingBoxList() + .forEach(bbs::add)); + 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); - Vec3d epos = entityPosition; - entity.setPosition(epos.x, epos.y + totalResponse.y, epos.z); - entityData.put(nbtMotionKey, VecHelper.writeNBT(totalResponse)); + if (!serverPlayer) { + 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); + 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 stream = + VoxelShapes.compare(voxelshape, VoxelShapes.create(bb.shrink(1.0E-7D)), IBooleanFunction.AND) + ? Stream.empty() + : Stream.of(voxelshape); + Stream stream1 = world.getEmptyCollisionShapes(e, bb.expand(movement), ImmutableSet.of()); + ReuseableStream 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); diff --git a/src/main/java/com/simibubi/create/foundation/collision/CollisionDebugger.java b/src/main/java/com/simibubi/create/foundation/collision/CollisionDebugger.java index 32270f0f1..b8c3ec26c 100644 --- a/src/main/java/com/simibubi/create/foundation/collision/CollisionDebugger.java +++ b/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 OrientedBB OBB = null; - static Vec3d seperation; + public static AxisAlignedBB AABB = new AxisAlignedBB(BlockPos.ZERO.up(10)); + public static OrientedBB OBB = new OrientedBB(new AxisAlignedBB(BlockPos.ZERO)); + public static Vec3d motion = Vec3d.ZERO; + static ContinuousSeparationManifold seperation; static double angle = 0; static AABBOutline outline; public static void onScroll(double delta) { -// angle += delta; -// movingBB = new OrientedBB(new AxisAlignedBB(BlockPos.ZERO).expand(0, 1, 0)); -// movingBB.setRotation(new Matrix3d().asZRotation(AngleHelper.rad(angle))); + angle += delta; + OBB.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) - return; - if (AABB == null) - return; - seperation = OBB.intersect(AABB); + AABB = new AxisAlignedBB(BlockPos.ZERO.up(60)).offset(.5, 0, .5); + motion = new Vec3d(0, -2, -.5f); + RayTraceResult mouse = Minecraft.getInstance().objectMouseOver; + if (mouse != null && mouse.getType() == Type.BLOCK) { + 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); + } + } diff --git a/src/main/java/com/simibubi/create/foundation/collision/ContinuousOBBCollider.java b/src/main/java/com/simibubi/create/foundation/collision/ContinuousOBBCollider.java new file mode 100644 index 000000000..4d951c985 --- /dev/null +++ b/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; + } + + } + +} diff --git a/src/main/java/com/simibubi/create/foundation/collision/Matrix3d.java b/src/main/java/com/simibubi/create/foundation/collision/Matrix3d.java index 49f31a2cd..05e24b0f2 100644 --- a/src/main/java/com/simibubi/create/foundation/collision/Matrix3d.java +++ b/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 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; + double x = vec.x * m00 + vec.y * m01 + vec.z * m02; + double y = vec.x * m10 + vec.y * m11 + vec.z * m12; + double z = vec.x * m20 + vec.y * m21 + vec.z * m22; return new Vec3d(x, y, z); } diff --git a/src/main/java/com/simibubi/create/foundation/collision/OBBCollider.java b/src/main/java/com/simibubi/create/foundation/collision/OBBCollider.java new file mode 100644 index 000000000..3d51e398c --- /dev/null +++ b/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)); + } + } + +} diff --git a/src/main/java/com/simibubi/create/foundation/collision/OrientedBB.java b/src/main/java/com/simibubi/create/foundation/collision/OrientedBB.java index 0c7a2e770..8b7c75225 100644 --- a/src/main/java/com/simibubi/create/foundation/collision/OrientedBB.java +++ b/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 org.apache.commons.lang3.mutable.MutableDouble; -import org.apache.commons.lang3.mutable.MutableObject; - -import com.simibubi.create.CreateClient; +import com.simibubi.create.foundation.collision.ContinuousOBBCollider.ContinuousSeparationManifold; 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 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 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) + } diff --git a/src/main/java/com/simibubi/create/foundation/utility/AngleHelper.java b/src/main/java/com/simibubi/create/foundation/utility/AngleHelper.java index 4469d2051..f42a9f6c0 100644 --- a/src/main/java/com/simibubi/create/foundation/utility/AngleHelper.java +++ b/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); }