From 56fe0c9c8af66d101b3df903231e2482dd90ad44 Mon Sep 17 00:00:00 2001 From: simibubi <31564874+simibubi@users.noreply.github.com> Date: Fri, 29 May 2020 17:59:56 +0200 Subject: [PATCH] S.A.T. will separate thee! - Implemented a prototype of a new collision resolver that supports rotated bounding boxes --- .../ContraptionCollider.java | 179 +++++++++++++----- .../structureMovement/ContraptionEntity.java | 7 +- .../bearing/MechanicalBearingTileEntity.java | 32 ++-- .../create/foundation/collision/Matrix3d.java | 117 ++++++++++++ .../foundation/collision/OrientedBB.java | 148 +++++++++++++++ 5 files changed, 417 insertions(+), 66 deletions(-) create mode 100644 src/main/java/com/simibubi/create/foundation/collision/Matrix3d.java create mode 100644 src/main/java/com/simibubi/create/foundation/collision/OrientedBB.java 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 768d7a906..8071ed2bb 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 @@ -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))) { - - ReuseableStream 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; + e -> canBeCollidedWith(e))) { if (entity instanceof PlayerEntity && !world.isRemote) return; - entity.setMotion(allowedMovement.add(contraptionMotion)); - entity.velocityChanged = true; + Vec3d centerOf = VecHelper.getCenterOf(BlockPos.ZERO); + 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 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) { - AxisAlignedBB entityBB = entity.getBoundingBox().offset(positionOffset); + Vec3d shapeMotion) { + 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() - : entityBB.getZSize() + shapeBB.getZSize(); + : axis == Axis.Y ? entityBB.getYSize() + shapeBB.getYSize() : 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 getPotentiallyCollidedShapes(World world, Contraption contraption, - Vec3d contraptionPosition, Entity entity) { - AxisAlignedBB blockScanBB = entity.getBoundingBox().offset(contraptionPosition.scale(-1)).grow(.5f); + AxisAlignedBB localBB) { + 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 potentialHits = - new ReuseableStream<>(BlockPos.getAllInBox(min, max).filter(contraption.blocks::containsKey).map(p -> { + ReuseableStream potentialHits = new ReuseableStream<>(BlockPos.getAllInBox(min, max) + .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() - && !collidedState.getCollisionShape(world, colliderPos).isEmpty()) { + if (!collidedState.getMaterial() + .isReplaceable() + && !collidedState.getCollisionShape(world, colliderPos) + .isEmpty()) { return true; } diff --git a/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/ContraptionEntity.java b/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/ContraptionEntity.java index af3528114..731e3cd51 100644 --- a/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/ContraptionEntity.java +++ b/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); + } } diff --git a/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/bearing/MechanicalBearingTileEntity.java b/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/bearing/MechanicalBearingTileEntity.java index fdd2ae3e8..f5463d1ad 100644 --- a/src/main/java/com/simibubi/create/content/contraptions/components/structureMovement/bearing/MechanicalBearingTileEntity.java +++ b/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 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) { diff --git a/src/main/java/com/simibubi/create/foundation/collision/Matrix3d.java b/src/main/java/com/simibubi/create/foundation/collision/Matrix3d.java new file mode 100644 index 000000000..473e97f13 --- /dev/null +++ b/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); + } + +} diff --git a/src/main/java/com/simibubi/create/foundation/collision/OrientedBB.java b/src/main/java/com/simibubi/create/foundation/collision/OrientedBB.java new file mode 100644 index 000000000..fc80f26d0 --- /dev/null +++ b/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 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 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; + } + +}