mirror of
https://github.com/Creators-of-Create/Create.git
synced 2024-09-21 08:58:55 +02:00
S.A.T. will separate thee!
- Implemented a prototype of a new collision resolver that supports rotated bounding boxes
This commit is contained in:
simibubi
parent
7b64f06d79
commit
56fe0c9c8a
5 changed files with 417 additions and 66 deletions
|
|
@ -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<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;
|
||||
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<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) {
|
||||
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<VoxelShape> 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<VoxelShape> potentialHits =
|
||||
new ReuseableStream<>(BlockPos.getAllInBox(min, max).filter(contraption.blocks::containsKey).map(p -> {
|
||||
ReuseableStream<VoxelShape> 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Reference in a new issue