Gen

.gitignore

@@ -29,3 +29,4 @@ Thumbs.db
 remappedSrc
 logs
 *.hprof
+generated

build.gradle

@@ -45,6 +45,10 @@ repositories {
 		name = "Ladysnake Libs"
 		url = "https://dl.bintray.com/ladysnake/libs"
 	}
+
+	maven {
+		url = "https://maven.shedaniel.me/"
+	}
 }
 
 def includeCompile(group, artifact, version) {
@@ -73,6 +77,17 @@ def includeCompile(group, artifact, version) {
 	}
 }
 
+sourceSets {
+	main {
+		java {
+			srcDir "src/main/schematics"
+			srcDir "src/main/registry"
+			srcDir "src/main/config"
+		}
+	}
+	datagen
+}
+
 dependencies {
 	minecraft "com.mojang:minecraft:21w06a"
 	mappings "net.fabricmc:yarn:21w06a+build.3:v2"
@@ -100,7 +115,12 @@ dependencies {
 	}
 	modCompileOnly 'com.github.badasintended:wthit:3.0.0'
 	modRuntime 'com.github.badasintended:wthit:3.0.0'
+
 	datagenImplementation sourceSets.main.output
+	datagenImplementation sourceSets.main.compileClasspath
+	datagenImplementation "me.shedaniel.cloth.api:cloth-datagen-api-v1:2.0.0"
+
+	testImplementation('org.junit.jupiter:junit-jupiter:5.5.2')
 }
 
 version = computeVersion(project.mod_version)
@@ -113,17 +133,6 @@ static def computeVersion(String version) {
 	return version
 }
 
-sourceSets {
-	main {
-		java {
-			srcDir "src/main/schematics"
-			srcDir "src/main/registry"
-			srcDir "src/main/config"
-		}
-	}
-	datagen
-}
-
 processResources {
 	filesMatching("fabric.mod.json") {
 		expand "version": project.version
@@ -156,3 +165,7 @@ curseforge {
 		forgeGradleIntegration = false
 	}
 }
+
+test {
+	useJUnitPlatform()
+}

src/datagen/java/org/dimdev/dimdoors/datagen/DatagenInitializer.java

@@ -0,0 +1,24 @@
+package org.dimdev.dimdoors.datagen;
+
+import java.nio.file.Paths;
+import java.util.Map;
+
+import me.shedaniel.cloth.api.datagen.v1.DataGeneratorHandler;
+import me.shedaniel.cloth.api.datagen.v1.RecipeData;
+import org.dimdev.dimdoors.block.ModBlocks;
+import org.dimdev.dimdoors.item.ModItems;
+
+import net.minecraft.block.Block;
+import net.minecraft.util.DyeColor;
+
+public class DatagenInitializer {
+	public static void main(String[] args) {
+		ModBlocks.init();
+		ModItems.init();
+		DataGeneratorHandler handler = DataGeneratorHandler.create(Paths.get("./generated"));
+		RecipeData recipes = handler.getRecipes();
+		for (Map.Entry<DyeColor, Block> entry : ModBlocks.FABRIC_BLOCKS.entrySet()) {
+			// TODO
+		}
+	}
+}

src/main/java/org/dimdev/dimdoors/block/CoordinateTransformerBlock.java

@@ -0,0 +1,41 @@
+package org.dimdev.dimdoors.block;
+
+import net.minecraft.block.BlockState;
+import net.minecraft.util.math.BlockPos;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
+import org.dimdev.dimdoors.util.math.TransformationMatrix3d;
+
+public interface CoordinateTransformerBlock {
+	TransformationMatrix3d.TransformationMatrix3dBuilder transformationBuilder(BlockState state, BlockPos pos);
+
+	TransformationMatrix3d.TransformationMatrix3dBuilder rotatorBuilder(BlockState state, BlockPos pos);
+
+	default Vec3d transformTo(TransformationMatrix3d.TransformationMatrix3dBuilder transformationBuilder, Vec3d vector) {
+		return transformationBuilder.build().transform(vector);
+	}
+
+	default Vec3d transformOut(TransformationMatrix3d.TransformationMatrix3dBuilder transformationBuilder, Vec3d vector) {
+		return transformationBuilder.buildReverse().transform(vector);
+	}
+
+	default EulerAngle rotateTo(TransformationMatrix3d.TransformationMatrix3dBuilder rotatorBuilder, EulerAngle angle) {
+		return rotatorBuilder.build().transform(angle);
+	}
+
+	default Vec3d rotateTo(TransformationMatrix3d.TransformationMatrix3dBuilder rotatorBuilder, Vec3d vector) {
+		return rotatorBuilder.build().transform(vector);
+	}
+
+	default EulerAngle rotateOut(TransformationMatrix3d.TransformationMatrix3dBuilder rotatorBuilder, EulerAngle angle) {
+		return rotatorBuilder.buildReverse().transform(angle);
+	}
+
+	default Vec3d rotateOut(TransformationMatrix3d.TransformationMatrix3dBuilder rotatorBuilder, Vec3d vector) {
+		return rotatorBuilder.buildReverse().transform(vector);
+	}
+
+	default boolean isExitFlipped() {
+		return false;
+	}
+}

src/main/java/org/dimdev/dimdoors/block/DimensionalDoorBlock.java

@@ -1,7 +1,10 @@
 package org.dimdev.dimdoors.block;
 
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.block.entity.DetachedRiftBlockEntity;
 import org.dimdev.dimdoors.block.entity.EntranceRiftBlockEntity;
+import org.dimdev.dimdoors.util.math.MathUtil;
+import org.dimdev.dimdoors.util.math.TransformationMatrix3d;
 import org.jetbrains.annotations.Nullable;
 
 import net.minecraft.block.BlockState;
@@ -22,18 +25,27 @@ import net.minecraft.util.shape.VoxelShapes;
 import net.minecraft.world.BlockView;
 import net.minecraft.world.World;
 
-public class DimensionalDoorBlock extends DoorBlock implements RiftProvider<EntranceRiftBlockEntity> {
+public class DimensionalDoorBlock extends DoorBlock implements RiftProvider<EntranceRiftBlockEntity>, CoordinateTransformerBlock {
 	public DimensionalDoorBlock(Settings settings) {
 		super(settings);
 	}
 
 	@Override
-	@SuppressWarnings("deprecation")
+	@SuppressWarnings("deprecation") // TODO: change from onEntityCollision to some method for checking if player crossed portal plane
 	public void onEntityCollision(BlockState state, World world, BlockPos pos, Entity entity) {
 		if (world.isClient) {
 			return;
 		}
 
+
+
+		// TODO: replace with dimdoor cooldown?
+		if (entity.hasNetherPortalCooldown()) {
+			entity.resetNetherPortalCooldown();
+			return;
+		}
+		entity.resetNetherPortalCooldown();
+
 		BlockState doorState = world.getBlockState(state.get(HALF) == DoubleBlockHalf.UPPER ? pos.down() : pos);
 
 		if (doorState.getBlock() == this && doorState.get(DoorBlock.OPEN)) { // '== this' to check if not half-broken
@@ -102,4 +114,22 @@ public class DimensionalDoorBlock extends DoorBlock implements RiftProvider<Entr
 	public VoxelShape getRaycastShape(BlockState state, BlockView world, BlockPos pos) {
 		return VoxelShapes.fullCube();
 	}
+
+	@Override
+	public TransformationMatrix3d.TransformationMatrix3dBuilder transformationBuilder(BlockState state, BlockPos pos) {
+		return TransformationMatrix3d.builder()
+				.inverseTranslate(Vec3d.ofCenter(pos).add(Vec3d.of(state.get(DoorBlock.FACING).getVector()).multiply(-0.5)))
+				.inverseRotate(MathUtil.directionEulerAngle(state.get(DoorBlock.FACING).getOpposite()));
+	}
+
+	@Override
+	public TransformationMatrix3d.TransformationMatrix3dBuilder rotatorBuilder(BlockState state, BlockPos pos) {
+		return TransformationMatrix3d.builder()
+				.inverseRotate(MathUtil.directionEulerAngle(state.get(DoorBlock.FACING).getOpposite()));
+	}
+
+	@Override
+	public boolean isExitFlipped() {
+		return true;
+	}
 }

src/main/java/org/dimdev/dimdoors/block/EternalFluidBlock.java

@@ -1,5 +1,6 @@
 package org.dimdev.dimdoors.block;
 
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.fluid.ModFluids;
 import org.dimdev.dimdoors.rift.targets.EntityTarget;
 import org.dimdev.dimdoors.rift.targets.EscapeTarget;
@@ -10,6 +11,7 @@ import net.minecraft.block.FluidBlock;
 import net.minecraft.entity.Entity;
 import net.minecraft.util.math.BlockPos;
 import net.minecraft.world.World;
+import org.dimdev.dimdoors.util.math.MathUtil;
 
 public class EternalFluidBlock extends FluidBlock {
 	private static final EntityTarget TARGET = new EscapeTarget(true);
@@ -25,7 +27,7 @@ public class EternalFluidBlock extends FluidBlock {
 		}
 
 		try {
-			TARGET.receiveEntity(entity, entity.yaw);
+			TARGET.receiveEntity(entity, Vec3d.ZERO, MathUtil.entityEulerAngle(entity), entity.getVelocity());
 		} catch (Throwable e) {
 			e.printStackTrace();
 		}

src/main/java/org/dimdev/dimdoors/block/ModBlocks.java

@@ -19,8 +19,8 @@ import net.fabricmc.fabric.api.object.builder.v1.block.FabricBlockSettings;
 
 public final class ModBlocks {
 	private static final Map<String, Block> BLOCKS = Maps.newLinkedHashMap();
-	private static final Map<DyeColor, Block> FABRIC_BLOCKS = new HashMap<>();
+	public static final Map<DyeColor, Block> FABRIC_BLOCKS = new HashMap<>();
-	private static final Map<DyeColor, Block> ANCIENT_FABRIC_BLOCKS = new HashMap<>();
+	public static final Map<DyeColor, Block> ANCIENT_FABRIC_BLOCKS = new HashMap<>();
 
 	public static final Block GOLD_DOOR = register("dimdoors:gold_door", new DoorBlock(FabricBlockSettings.of(Material.METAL, MapColor.GOLD).nonOpaque()));
 	public static final Block QUARTZ_DOOR = register("dimdoors:quartz_door", new DoorBlock(FabricBlockSettings.of(Material.STONE, MapColor.OFF_WHITE).nonOpaque()));

src/main/java/org/dimdev/dimdoors/block/entity/DetachedRiftBlockEntity.java

@@ -2,6 +2,8 @@ package org.dimdev.dimdoors.block.entity;
 
 import java.util.Random;
 
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.DimensionalDoorsInitializer;
 import org.dimdev.dimdoors.block.ModBlocks;
 import org.dimdev.dimdoors.util.TeleportUtil;
@@ -118,9 +120,9 @@ public class DetachedRiftBlockEntity extends RiftBlockEntity {
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d velocity) {
 		if (this.world instanceof ServerWorld)
-			TeleportUtil.teleport(entity, this.world, this.pos, 0);
+			TeleportUtil.teleport(entity, this.world, this.pos, relativeAngle);
 		return true;
 	}
 

src/main/java/org/dimdev/dimdoors/block/entity/EntranceRiftBlockEntity.java

@@ -2,7 +2,12 @@ package org.dimdev.dimdoors.block.entity;
 
 import java.util.Optional;
 
+import net.minecraft.block.Block;
+import net.minecraft.network.packet.s2c.play.EntityVelocityUpdateS2CPacket;
+import net.minecraft.server.network.ServerPlayerEntity;
+import net.minecraft.util.math.EulerAngle;
 import org.dimdev.dimdoors.DimensionalDoorsInitializer;
+import org.dimdev.dimdoors.block.CoordinateTransformerBlock;
 import org.dimdev.dimdoors.util.TeleportUtil;
 
 import net.minecraft.block.BlockState;
@@ -13,6 +18,7 @@ import net.minecraft.nbt.CompoundTag;
 import net.minecraft.util.math.BlockPos;
 import net.minecraft.util.math.Direction;
 import net.minecraft.util.math.Vec3d;
+import org.dimdev.dimdoors.util.math.TransformationMatrix3d;
 
 public class EntranceRiftBlockEntity extends RiftBlockEntity {
 	public EntranceRiftBlockEntity(BlockPos pos, BlockState state) {
@@ -42,9 +48,42 @@ public class EntranceRiftBlockEntity extends RiftBlockEntity {
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
+		BlockState state = this.getWorld().getBlockState(this.getPos());
+		Block block = state.getBlock();
 		Vec3d targetPos = Vec3d.ofCenter(this.pos).add(Vec3d.of(this.getOrientation().getOpposite().getVector()).multiply(DimensionalDoorsInitializer.getConfig().getGeneralConfig().teleportOffset + 0.5));
-		TeleportUtil.teleport(entity, this.world, targetPos, yawOffset);
+
+		if (block instanceof CoordinateTransformerBlock) {
+			CoordinateTransformerBlock transformer = (CoordinateTransformerBlock) block;
+
+			System.out.println("{yaw: " + relativeAngle.getYaw() + "; pitch: " + relativeAngle.getPitch() + "; roll: " + relativeAngle.getRoll() + "}");
+
+			if (transformer.isExitFlipped()) {
+				TransformationMatrix3d flipper = TransformationMatrix3d.builder().rotateY(Math.PI).build();
+
+				relativePos = flipper.transform(relativePos);
+				relativeAngle = flipper.transform(relativeAngle);
+				relativeVelocity = flipper.transform(relativeVelocity);
+			}
+
+			System.out.println("{yaw: " + relativeAngle.getYaw() + "; pitch: " + relativeAngle.getPitch() + "; roll: " + relativeAngle.getRoll() + "}");
+
+			relativePos = relativePos.add(new Vec3d(0, 0, 1).multiply(0.6)); // TODO: skip this for Immersive Portals
+
+			TransformationMatrix3d.TransformationMatrix3dBuilder transformationBuilder = transformer.transformationBuilder(state, this.getPos());
+			TransformationMatrix3d.TransformationMatrix3dBuilder rotatorBuilder = transformer.rotatorBuilder(state, this.getPos());
+			targetPos = transformer.transformOut(transformationBuilder, relativePos);
+			relativeAngle = transformer.rotateOut(rotatorBuilder, relativeAngle);
+			relativeVelocity = transformer.rotateOut(rotatorBuilder, relativeVelocity);
+		}
+
+		TeleportUtil.teleport(entity, this.world, targetPos, relativeAngle);
+		entity.setVelocity(relativeVelocity);
+		if (entity instanceof ServerPlayerEntity) {
+			ServerPlayerEntity player = (ServerPlayerEntity) entity;
+			player.networkHandler.sendPacket(new EntityVelocityUpdateS2CPacket(player));
+		}
+
 		return true;
 	}
 
@@ -56,6 +95,10 @@ public class EntranceRiftBlockEntity extends RiftBlockEntity {
 				.orElse(Direction.NORTH);
 	}
 
+	public boolean hasOrientation() {
+		return this.world != null && this.world.getBlockState(this.pos).contains(HorizontalFacingBlock.FACING);
+	}
+
 	/**
 	 * Specifies if the portal should be rendered two blocks tall
 	 */

src/main/java/org/dimdev/dimdoors/block/entity/RiftBlockEntity.java

@@ -2,8 +2,12 @@ package org.dimdev.dimdoors.block.entity;
 
 import java.util.Objects;
 
+import net.minecraft.block.Block;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
+import org.dimdev.dimdoors.block.CoordinateTransformerBlock;
 import org.dimdev.dimdoors.pockets.PocketTemplate;
 import org.dimdev.dimdoors.rift.registry.LinkProperties;
 import org.dimdev.dimdoors.rift.registry.Rift;
@@ -15,6 +19,7 @@ import org.dimdev.dimdoors.rift.targets.VirtualTarget;
 import org.dimdev.dimdoors.util.EntityUtils;
 import org.dimdev.dimdoors.util.Location;
 import org.dimdev.dimdoors.util.RGBA;
+import org.dimdev.dimdoors.util.math.TransformationMatrix3d;
 import org.dimdev.dimdoors.world.level.DimensionalRegistry;
 import org.dimdev.dimdoors.world.pocket.VirtualLocation;
 import org.jetbrains.annotations.NotNull;
@@ -173,9 +178,23 @@ public abstract class RiftBlockEntity extends BlockEntity implements BlockEntity
 
 		// Attempt a teleport
 		try {
+			Vec3d relativePos = new Vec3d(0, 0, 0);
+			EulerAngle relativeAngle = new EulerAngle(entity.pitch, entity.yaw, 0);
+			Vec3d relativeVelocity = entity.getVelocity();
 			EntityTarget target = this.getTarget().as(Targets.ENTITY);
 
-			if (target.receiveEntity(entity, entity.yaw)) {
+			BlockState state = this.getWorld().getBlockState(this.getPos());
+			Block block = state.getBlock();
+			if (block instanceof CoordinateTransformerBlock) {
+				CoordinateTransformerBlock transformer = (CoordinateTransformerBlock) block;
+				TransformationMatrix3d.TransformationMatrix3dBuilder transformationBuilder = transformer.transformationBuilder(state, this.getPos());
+				TransformationMatrix3d.TransformationMatrix3dBuilder rotatorBuilder = transformer.rotatorBuilder(state, this.getPos());
+				relativePos = transformer.transformTo(transformationBuilder, entity.getPos());
+				relativeAngle = transformer.rotateTo(rotatorBuilder, relativeAngle);
+				relativeVelocity = transformer.rotateTo(rotatorBuilder, relativeVelocity);
+			}
+
+			if (target.receiveEntity(entity, relativePos, relativeAngle, relativeVelocity)) {
 				VirtualLocation vLoc = VirtualLocation.fromLocation(new Location((ServerWorld) entity.world, entity.getBlockPos()));
 				EntityUtils.chat(entity, new LiteralText("You are at x = " + vLoc.getX() + ", y = ?, z = " + vLoc.getZ() + ", w = " + vLoc.getDepth()));
 				return true;

src/main/java/org/dimdev/dimdoors/block/entity/RiftData.java

@@ -16,7 +16,7 @@ public class RiftData {
 	private LinkProperties properties = null;
 	private boolean alwaysDelete;
 	private boolean forcedColor;
-	private RGBA color = null;
+	private RGBA color = RGBA.NONE;
 
 	public RiftData() {
 	}
@@ -78,7 +78,7 @@ public class RiftData {
 		data.properties = tag.contains("properties") ? LinkProperties.fromTag(tag.getCompound("properties")) : null;
 		data.alwaysDelete = tag.getBoolean("alwaysDelete");
 		data.forcedColor = tag.getBoolean("forcedColor");
-		data.color = tag.contains("color") ? RGBA.fromTag(tag.getCompound("color")) : null;
+		data.color = tag.contains("color") ? RGBA.fromTag(tag.getCompound("color")) : RGBA.NONE;
 		return data;
 	}
 

src/main/java/org/dimdev/dimdoors/client/DetachedRiftBlockEntityRenderer.java

@@ -21,7 +21,7 @@ import net.fabricmc.api.Environment;
 @Environment(EnvType.CLIENT)
 public class DetachedRiftBlockEntityRenderer implements BlockEntityRenderer<DetachedRiftBlockEntity> {
     public static final Identifier TESSERACT_PATH = new Identifier("dimdoors:textures/other/tesseract.png");
-    private final RGBA color = new RGBA(1, 0.5f, 1, 1);
+    private static final RGBA DEFAULT_COLOR = new RGBA(1, 0.5f, 1, 1);
 
     private static final Tesseract TESSERACT = new Tesseract();
     private static final RiftCurves.PolygonInfo CURVE = RiftCurves.CURVES.get(0);
@@ -51,7 +51,7 @@ public class DetachedRiftBlockEntityRenderer implements BlockEntityRenderer<Deta
         double radian = this.nextAngle(rift, tickDelta) * TrigMath.DEG_TO_RAD;
         RGBA color = rift.getColor();
         if (Objects.equals(color, RGBA.NONE)) {
-            color = this.color;
+            color = DEFAULT_COLOR;
         }
 
         matrices.push();

src/main/java/org/dimdev/dimdoors/command/DimTeleportCommand.java

@@ -12,6 +12,7 @@ import net.minecraft.server.command.ServerCommandSource;
 import net.minecraft.server.network.ServerPlayerEntity;
 import net.minecraft.server.world.ServerWorld;
 import net.minecraft.util.math.Vec3d;
+import org.dimdev.dimdoors.util.math.MathUtil;
 
 public class DimTeleportCommand {
     public static void register(CommandDispatcher<ServerCommandSource> dispatcher) {
@@ -33,7 +34,7 @@ public class DimTeleportCommand {
     }
 
     private static int teleport(Entity entity, ServerWorld dimension, Vec3d pos) {
-		TeleportUtil.teleport(entity, dimension, pos, 0);
+		TeleportUtil.teleport(entity, dimension, pos, MathUtil.entityEulerAngle(entity));
         return Command.SINGLE_SUCCESS;
     }
 }

src/main/java/org/dimdev/dimdoors/command/PocketCommand.java

@@ -5,6 +5,8 @@ import com.mojang.brigadier.CommandDispatcher;
 import com.mojang.brigadier.arguments.BoolArgumentType;
 import com.mojang.brigadier.context.CommandContext;
 import com.mojang.brigadier.exceptions.CommandSyntaxException;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.command.arguments.GroupArugmentType;
 import org.dimdev.dimdoors.command.arguments.NameArugmentType;
 import org.dimdev.dimdoors.pockets.PocketGenerator;
@@ -64,7 +66,7 @@ public class PocketCommand {
             if (DimensionalRegistry.getRiftRegistry().getPocketEntrance(pocket) != null) {
                 EntityTarget entrance = (EntityTarget) player.world.getBlockEntity(DimensionalRegistry.getRiftRegistry().getPocketEntrance(pocket).pos);
                 if (entrance != null) {
-                    entrance.receiveEntity(player, 0);
+                    entrance.receiveEntity(player, Vec3d.ZERO, new EulerAngle(0, 0, 0), player.getVelocity());
                 }
             } else {
                 Vector3i size = pocket.getSize().add(1, 1, 1).mul(15).div(2);

src/main/java/org/dimdev/dimdoors/pockets/generator/ChunkGenerator.java

@@ -173,10 +173,11 @@ public class ChunkGenerator extends PocketGenerator {
 			}
 		}
 		Box virtualBox = realBox.offset(pocketOriginChunkOffset.add(0, virtualYOffset, 0));
+		/*
 		for (Entity entity : protoRegion.getOtherEntities(null, virtualBox)) { // TODO: does this even work?
 			TeleportUtil.teleport(entity, world, entity.getPos().add(-pocketOriginChunkOffset.getX(), -pocketOriginChunkOffset.getY() - virtualYOffset, -pocketOriginChunkOffset.getZ()), entity.yaw);
 		} // TODO: Entities?/ Biomes/ Structure Data
-
+		*/
 		world.setBlockState(world.getTopPosition(Heightmap.Type.MOTION_BLOCKING_NO_LEAVES, pocket.getOrigin()), ModBlocks.DETACHED_RIFT.getDefaultState());
 
 		DetachedRiftBlockEntity rift = ModBlockEntityTypes.DETACHED_RIFT.instantiate(world.getTopPosition(Heightmap.Type.MOTION_BLOCKING_NO_LEAVES, pocket.getOrigin()), ModBlocks.DETACHED_RIFT.getDefaultState());

src/main/java/org/dimdev/dimdoors/rift/targets/EntityTarget.java

@@ -1,7 +1,9 @@
 package org.dimdev.dimdoors.rift.targets;
 
 import net.minecraft.entity.Entity;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 
 public interface EntityTarget extends Target {
-	boolean receiveEntity(Entity entity, float yawOffset);
+	boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity);
 }

src/main/java/org/dimdev/dimdoors/rift/targets/EscapeTarget.java

@@ -4,6 +4,8 @@ import java.util.UUID;
 
 import com.mojang.serialization.Codec;
 import com.mojang.serialization.codecs.RecordCodecBuilder;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.block.entity.RiftBlockEntity;
 import org.dimdev.dimdoors.util.Location;
 import org.dimdev.dimdoors.util.TeleportUtil;
@@ -31,7 +33,7 @@ public class EscapeTarget extends VirtualTarget implements EntityTarget { // TOD
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
 		if (!ModDimensions.isPocketDimension(entity.world) && !(ModDimensions.isLimboDimension(entity.world))) {
 			chat(entity, new TranslatableText("rifts.destinations.escape.not_in_pocket_dim"));
 			return false;
@@ -46,7 +48,7 @@ public class EscapeTarget extends VirtualTarget implements EntityTarget { // TOD
 			Location destLoc = DimensionalRegistry.getRiftRegistry().getOverworldRift(uuid);
 			if (destLoc != null && destLoc.getBlockEntity() instanceof RiftBlockEntity || this.canEscapeLimbo) {
 				Location location = VirtualLocation.fromLocation(new Location((ServerWorld) entity.world, entity.getBlockPos())).projectToWorld(false);
-				TeleportUtil.teleport(entity, location.getWorld(), location.getBlockPos(), 0);
+				TeleportUtil.teleport(entity, location.getWorld(), location.getBlockPos(), relativeAngle);
 			} else {
 				if (destLoc == null) {
 					chat(entity, new TranslatableText("rifts.destinations.escape.did_not_use_rift"));
@@ -54,7 +56,7 @@ public class EscapeTarget extends VirtualTarget implements EntityTarget { // TOD
 					chat(entity, new TranslatableText("rifts.destinations.escape.rift_has_closed"));
 				}
 				if (ModDimensions.LIMBO_DIMENSION != null) {
-					TeleportUtil.teleport(entity, ModDimensions.LIMBO_DIMENSION, new BlockPos(this.location.getX(), this.location.getY(), this.location.getZ()), entity.getYaw(1.0F));
+					TeleportUtil.teleport(entity, ModDimensions.LIMBO_DIMENSION, new BlockPos(this.location.getX(), this.location.getY(), this.location.getZ()), relativeAngle);
 				}
 			}
 			return true;

src/main/java/org/dimdev/dimdoors/rift/targets/IdMarker.java

@@ -1,5 +1,7 @@
 package org.dimdev.dimdoors.rift.targets;
 
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.util.EntityUtils;
 
 import net.minecraft.entity.Entity;
@@ -33,7 +35,7 @@ public class IdMarker extends VirtualTarget implements EntityTarget {
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
 		EntityUtils.chat(entity, Text.of("This rift is configured for pocket dungeons. Its id is " + this.id));
 		return false;
 	}

src/main/java/org/dimdev/dimdoors/rift/targets/LimboTarget.java

@@ -1,6 +1,8 @@
 package org.dimdev.dimdoors.rift.targets;
 
 import com.mojang.serialization.Codec;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.util.TeleportUtil;
 import org.dimdev.dimdoors.world.ModDimensions;
 
@@ -14,8 +16,8 @@ public class LimboTarget extends VirtualTarget implements EntityTarget {
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
-		TeleportUtil.teleport(entity, ModDimensions.LIMBO_DIMENSION, entity.getPos(), yawOffset);
+		TeleportUtil.teleport(entity, ModDimensions.LIMBO_DIMENSION, entity.getPos(), relativeAngle);
 		return true;
 	}
 

src/main/java/org/dimdev/dimdoors/rift/targets/MessageTarget.java

@@ -1,5 +1,7 @@
 package org.dimdev.dimdoors.rift.targets;
 
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.util.EntityUtils;
 
 import net.minecraft.entity.Entity;
@@ -23,11 +25,11 @@ public class MessageTarget implements EntityTarget {
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
 		EntityUtils.chat(entity, new TranslatableText(this.message, this.messageParams));
 
 		if (this.forwardTo != null) {
-			this.forwardTo.as(Targets.ENTITY).receiveEntity(entity, yawOffset);
+			this.forwardTo.as(Targets.ENTITY).receiveEntity(entity, relativePos, relativeAngle, relativeVelocity);
 			return true;
 		} else {
 			return false;

src/main/java/org/dimdev/dimdoors/rift/targets/PocketEntranceMarker.java

@@ -1,5 +1,7 @@
 package org.dimdev.dimdoors.rift.targets;
 
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.util.EntityUtils;
 
 import net.minecraft.entity.Entity;
@@ -27,7 +29,7 @@ public class PocketEntranceMarker extends VirtualTarget implements EntityTarget
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
 		EntityUtils.chat(entity, new TranslatableText("The entrance of this dungeon has not been converted. If this is a normally generated pocket, please report this bug."));
 		return false;
 	}

src/main/java/org/dimdev/dimdoors/rift/targets/PocketExitMarker.java

@@ -1,6 +1,8 @@
 package org.dimdev.dimdoors.rift.targets;
 
 import com.mojang.serialization.Codec;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.util.EntityUtils;
 
 import net.minecraft.entity.Entity;
@@ -13,7 +15,7 @@ public class PocketExitMarker extends VirtualTarget implements EntityTarget {
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
 		EntityUtils.chat(entity, new TranslatableText("The exit of this dungeon has not been linked. If this is a normally generated pocket, please report this bug."));
 		return false;
 	}

src/main/java/org/dimdev/dimdoors/rift/targets/PrivatePocketExitTarget.java

@@ -2,6 +2,8 @@ package org.dimdev.dimdoors.rift.targets;
 
 import java.util.UUID;
 
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.dimdev.dimdoors.block.entity.RiftBlockEntity;
 import org.dimdev.dimdoors.util.EntityUtils;
 import org.dimdev.dimdoors.util.Location;
@@ -23,7 +25,7 @@ public class PrivatePocketExitTarget extends VirtualTarget implements EntityTarg
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
 		Location destLoc;
 		// TODO: make this recursive
 		UUID uuid = EntityUtils.getOwner(entity).getUuid();
@@ -41,7 +43,7 @@ public class PrivatePocketExitTarget extends VirtualTarget implements EntityTarg
 				}
 				return false;
 			} else {
-				((EntityTarget) destLoc.getBlockEntity()).receiveEntity(entity, yawOffset);
+				((EntityTarget) destLoc.getBlockEntity()).receiveEntity(entity, relativePos, relativeAngle, relativeVelocity);
 				return true;
 			}
 		} else {

src/main/java/org/dimdev/dimdoors/rift/targets/PrivatePocketTarget.java

@@ -2,6 +2,8 @@ package org.dimdev.dimdoors.rift.targets;
 
 import java.util.UUID;
 
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 import org.dimdev.dimdoors.pockets.PocketGenerator;
@@ -27,7 +29,7 @@ public class PrivatePocketTarget extends VirtualTarget implements EntityTarget {
 	}
 
 	@Override
-	public boolean receiveEntity(Entity entity, float yawOffset) {
+	public boolean receiveEntity(Entity entity, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
 		// TODO: make this recursive
 		UUID uuid = EntityUtils.getOwner(entity).getUuid();
 		VirtualLocation virtualLocation = VirtualLocation.fromLocation(this.location);
@@ -39,7 +41,7 @@ public class PrivatePocketTarget extends VirtualTarget implements EntityTarget {
 
 				DimensionalRegistry.getPrivateRegistry().setPrivatePocketID(uuid, pocket);
 				BlockEntity be = DimensionalRegistry.getRiftRegistry().getPocketEntrance(pocket).getBlockEntity();
-				this.processEntity(pocket, be, entity, uuid, yawOffset);
+				this.processEntity(pocket, be, entity, uuid, relativePos, relativeAngle, relativeVelocity);
 			} else {
 				Location destLoc = DimensionalRegistry.getRiftRegistry().getPrivatePocketEntrance(uuid); // get the last used entrances
 				if (destLoc == null)
@@ -52,7 +54,7 @@ public class PrivatePocketTarget extends VirtualTarget implements EntityTarget {
 					destLoc = DimensionalRegistry.getRiftRegistry().getPocketEntrance(pocket);
 				}
 
-				this.processEntity(pocket, destLoc.getBlockEntity(), entity, uuid, yawOffset);
+				this.processEntity(pocket, destLoc.getBlockEntity(), entity, uuid, relativePos, relativeAngle, relativeVelocity);
 			}
 			return true;
 		} else {
@@ -60,7 +62,7 @@ public class PrivatePocketTarget extends VirtualTarget implements EntityTarget {
 		}
 	}
 
-	private void processEntity(Pocket pocket, BlockEntity blockEntity, Entity entity, UUID uuid, float relativeYaw) {
+	private void processEntity(Pocket pocket, BlockEntity blockEntity, Entity entity, UUID uuid, Vec3d relativePos, EulerAngle relativeAngle, Vec3d relativeVelocity) {
 		if (entity instanceof ItemEntity) {
 			Item item = ((ItemEntity) entity).getStack().getItem();
 
@@ -68,13 +70,13 @@ public class PrivatePocketTarget extends VirtualTarget implements EntityTarget {
 				if (pocket.addDye(EntityUtils.getOwner(entity), ((DyeItem) item).getColor())) {
 					entity.remove(Entity.RemovalReason.DISCARDED);
 				} else {
-					((EntityTarget) blockEntity).receiveEntity(entity, relativeYaw);
+					((EntityTarget) blockEntity).receiveEntity(entity, relativePos, relativeAngle, relativeVelocity);
 				}
 			} else {
-				((EntityTarget) blockEntity).receiveEntity(entity, relativeYaw);
+				((EntityTarget) blockEntity).receiveEntity(entity, relativePos, relativeAngle, relativeVelocity);
 			}
 		} else {
-			((EntityTarget) blockEntity).receiveEntity(entity, relativeYaw);
+			((EntityTarget) blockEntity).receiveEntity(entity, relativePos, relativeAngle, relativeVelocity);
 			DimensionalRegistry.getRiftRegistry().setLastPrivatePocketExit(uuid, this.location);
 		}
 	}

src/main/java/org/dimdev/dimdoors/rift/targets/Targets.java

@@ -14,7 +14,7 @@ public final class Targets {
 	public static final Class<RedstoneTarget> REDSTONE = RedstoneTarget.class;
 
 	public static void registerDefaultTargets() {
-		DefaultTargets.registerDefaultTarget(ENTITY, (entity, relativeYaw) -> {
+		DefaultTargets.registerDefaultTarget(ENTITY, (entity, relativePos, relativeRotation, relativeVelocity) -> {
 			EntityUtils.chat(entity, new TranslatableText("rifts.unlinked2"));
 			return false;
 		});

src/main/java/org/dimdev/dimdoors/util/TeleportUtil.java

@@ -2,6 +2,7 @@ package org.dimdev.dimdoors.util;
 
 import java.util.concurrent.ThreadLocalRandom;
 
+import net.minecraft.util.math.EulerAngle;
 import org.dimdev.dimdoors.DimensionalDoorsInitializer;
 
 import net.minecraft.entity.Entity;
@@ -13,6 +14,7 @@ import net.minecraft.world.TeleportTarget;
 import net.minecraft.world.World;
 
 import net.fabricmc.fabric.api.dimension.v1.FabricDimensions;
+import org.dimdev.dimdoors.util.math.MathUtil;
 
 @SuppressWarnings("deprecation")
 public final class TeleportUtil {
@@ -32,6 +34,22 @@ public final class TeleportUtil {
 		FabricDimensions.teleport(entity, (ServerWorld) world, new TeleportTarget(pos, entity.getVelocity(), yaw, entity.getPitch(1.0F)));
 	}
 
+	public static void teleport(Entity entity, World world, Vec3d pos, EulerAngle angle) {
+		if (world.isClient) {
+			throw new UnsupportedOperationException("Only supported on ServerWorld");
+		}
+
+		FabricDimensions.teleport(entity, (ServerWorld) world, new TeleportTarget(pos, entity.getVelocity(), angle.getYaw(), angle.getPitch()));
+	}
+
+	public static void teleport(Entity entity, World world, BlockPos pos, EulerAngle angle) {
+		if (world.isClient) {
+			throw new UnsupportedOperationException("Only supported on ServerWorld");
+		}
+
+		teleport(entity, world, Vec3d.ofBottomCenter(pos), angle);
+	}
+
 	public static void teleport(ServerPlayerEntity player, Location location) {
 		teleport(player, DimensionalDoorsInitializer.getWorld(location.world), location.pos, 0);
 	}

src/main/java/org/dimdev/dimdoors/util/math/Equation.java

@@ -62,7 +62,7 @@ public interface Equation {
 
 			// &&
 			Map<String, TriFunction<Map<String, Double>, Equation, Equation, Double>> and = new HashMap<>();
-			and.put("&&", (stringDoubleMap, first, second) -> toDouble(first.asBoolean(stringDoubleMap) || second.asBoolean(stringDoubleMap)));
+			and.put("&&", (stringDoubleMap, first, second) -> toDouble(first.asBoolean(stringDoubleMap) && second.asBoolean(stringDoubleMap)));
 			parseRules.add(new SplitterParser(and));
 
 			// ==, <=, >=, <, >

src/main/java/org/dimdev/dimdoors/util/math/MathUtil.java

@@ -1,5 +1,11 @@
 package org.dimdev.dimdoors.util.math;
 
+import net.minecraft.entity.Entity;
+import net.minecraft.util.math.Direction;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.MathHelper;
+import net.minecraft.util.math.Vec3d;
+
 import java.util.Map;
 import java.util.Random;
 
@@ -18,4 +24,64 @@ public final class MathUtil {
 		}
 		return null;
 	}
+
+	public static EulerAngle eulerAngle(Vec3d direction, Vec3d upwards) {
+		float pitch = pitch(direction);
+		float yaw = yaw(direction);
+		upwards = TransformationMatrix3d.builder().rotate(new EulerAngle(pitch, yaw, 0)).buildReverse().transform(upwards);
+		float roll = (float) Math.toDegrees(-Math.atan2(upwards.x, upwards.y));
+
+		return new EulerAngle(pitch, yaw, roll);
+	}
+
+	public static EulerAngle entityEulerAngle(Entity entity) {
+		return new EulerAngle(entity.pitch, entity.yaw, 0);
+	}
+
+	public static float yaw(Vec3d vector) {
+		return (float) Math.toDegrees(-Math.atan2(vector.x, vector.z));
+	}
+
+	public static float pitch(Vec3d vector) {
+		return (float) Math.toDegrees(Math.asin(-vector.y));
+	}
+
+	public static EulerAngle directionEulerAngle(Direction direction) {
+		switch (direction) {
+			case DOWN:
+				return EulerAngleDirection.DOWN.getAngle();
+			case UP:
+				return EulerAngleDirection.UP.getAngle();
+			case NORTH:
+				return EulerAngleDirection.NORTH.getAngle();
+			case SOUTH:
+				return EulerAngleDirection.SOUTH.getAngle();
+			case WEST:
+				return EulerAngleDirection.WEST.getAngle();
+			case EAST:
+			default:
+				return EulerAngleDirection.EAST.getAngle();
+		}
+	}
+
+	public enum EulerAngleDirection {
+		DOWN(new EulerAngle(90, 0, 0)),
+		UP(new EulerAngle(-90, 0, 0)),
+		NORTH(new EulerAngle(0, -180, 0)),
+		SOUTH(new EulerAngle(0, 0, 0)),
+		WEST(new EulerAngle(0, 90, 0)),
+		EAST(new EulerAngle(0, -90, 0));
+
+
+
+		private final EulerAngle angle;
+
+		EulerAngleDirection(EulerAngle angle) {
+			this.angle = angle;
+		}
+
+		public EulerAngle getAngle() {
+			return angle;
+		}
+	}
 }

src/main/java/org/dimdev/dimdoors/util/math/Matrixd.java

@@ -0,0 +1,46 @@
+package org.dimdev.dimdoors.util.math;
+
+public class Matrixd extends MatrixdImpl<Matrixd> {
+	public static Matrixd identity(int i, int j) {
+		double[][] identityMatrix = new double[i][j];
+		for (int n = 0; n < i && n < j; n++) {
+			identityMatrix[n][n] = 1;
+		}
+		return new Matrixd(identityMatrix);
+	}
+
+	public static Matrixd diag(double... diagEntries) {
+		double[][] diagMatrix = new double[diagEntries.length][diagEntries.length];
+		for (int i = 0; i < diagEntries.length; i++) {
+			diagMatrix[i][i] = diagEntries[i];
+		}
+		return new Matrixd(diagMatrix);
+	}
+
+	public Matrixd(double[][] matrix) {
+		super(matrix);
+	}
+
+	public Matrixd(MatrixdImpl<? extends MatrixdImpl<?>> matrixd) {
+		super(matrixd);
+	}
+
+	public Matrixd(Vectord... vectors) {
+		super(vectors);
+	}
+
+	@Override
+	public Matrixd construct(double[][] matrix) {
+		return new Matrixd(matrix);
+	}
+
+	@Override
+	public Matrixd construct(MatrixdImpl<? extends MatrixdImpl<?>> matrixd) {
+		return new Matrixd(matrixd);
+	}
+
+	@Override
+	public Matrixd construct(Vectord... vectors) {
+		return new Matrixd(vectors);
+	}
+}

src/main/java/org/dimdev/dimdoors/util/math/MatrixdImpl.java

@@ -0,0 +1,230 @@
+package org.dimdev.dimdoors.util.math;
+
+import java.util.Arrays;
+
+public abstract class MatrixdImpl<T extends MatrixdImpl<T>> {
+	private final int dimensionX;
+	private final int dimensionY;
+
+	protected double[][] matrix;
+
+	public MatrixdImpl(double[][] matrix) {
+		if (matrix.length > 0) { // Allow matrices of dimension 0x0. Why? No reason not to.
+			int length = matrix[0].length;
+			for (int i = 1; i < matrix.length; i++) {
+				if (length != matrix[i].length) throw new UnsupportedOperationException("Cannot create Matrix from 2D array consisting of non equal length arrays.");
+			}
+		}
+		this.matrix = matrix;
+
+		this.dimensionX = matrix.length;
+		this.dimensionY = matrix[0].length;
+	}
+
+	public MatrixdImpl(MatrixdImpl<?> matrixd) {
+		this.matrix = matrixd.getMatrix();
+
+		this.dimensionX = matrixd.getDimensionX();
+		this.dimensionY = matrixd.getDimensionY();
+	}
+
+	public MatrixdImpl(Vectord... vectors) {
+		double[][] matrix = new double[vectors.length][vectors[0].size()];
+		for (int i = 0; i < vectors.length; i++) {
+			matrix[i] = vectors[i].getVec();
+		}
+
+		int length = matrix[0].length;
+		for (int i = 1; i < matrix.length; i++) {
+			if (length != matrix[i].length) throw new UnsupportedOperationException("Cannot create Matrix from 2D array consisting of non equal length arrays.");
+		}
+
+		this.matrix = matrix;
+
+		this.dimensionX = matrix.length;
+		this.dimensionY = matrix[0].length;
+	}
+
+	public abstract T construct(double[][] matrix);
+
+	public abstract T construct(MatrixdImpl<?> matrixd);
+
+	public abstract T construct(Vectord... vectors);
+
+	public int getDimensionX() {
+		return dimensionX;
+	}
+
+	public int getDimensionY() {
+		return dimensionY;
+	}
+
+	private double[][] getMatrix() {
+		return matrix;
+	}
+
+	public double get(int column, int row) {
+		return matrix[column][row];
+	}
+
+	public Vectord getColumn(int column) {
+		return new Vectord(matrix[column]);
+	}
+
+	public Vectord getRow(int row) {
+		double[] rowArray = new double[dimensionX];
+		for (int i = 0; i < dimensionX; i++) {
+			rowArray[i] = matrix[i][row];
+		}
+		return new Vectord(rowArray);
+	}
+
+	public T set(int column, int row, double value) {
+		T updated = construct(this);
+		updated.matrix[column][row] = value;
+		return updated;
+	}
+
+	public T setColumn(int column, Vectord vector) {
+		if (vector.size() != this.dimensionY) throw new UnsupportedOperationException("Cannot replace column with one of non matching length");
+		T updated = construct(this);
+		updated.matrix[column] = vector.getVec();
+		return updated;
+	}
+
+	public T setRow(int row, Vectord vector) {
+		if (vector.size() != this.dimensionX) throw new UnsupportedOperationException("Cannot replace row with one of non matching length");
+		T updated = construct(this);
+		for (int i = 0; i < vector.size(); i++) {
+			updated.matrix[i][row] = vector.get(i);
+		}
+		return updated;
+	}
+
+	public T dropColumn(int column) {
+		double[][] matrix = new double[this.dimensionX - 1][this.dimensionY];
+		for (int i = 0; i < this.dimensionX; i++) {
+			if (i == column) continue;
+			matrix[i < column? i : i - 1] = this.matrix[i];
+		}
+		return construct(matrix);
+	}
+
+	public T dropRow(int row) {
+		double[][] matrix = new double[this.dimensionX - 1][this.dimensionY];
+		for (int i = 0; i < this.dimensionX; i++) {
+			for (int j = 0; j < this.dimensionY; j++) {
+				if (j == row) continue;
+				matrix[i][j < row? j : j - 1] = this.matrix[i][j];
+			}
+		}
+		return construct(matrix);
+	}
+
+	public T dropColumnAndRow(int column, int row) {
+		double[][] matrix = new double[this.dimensionX - 1][this.dimensionY];
+		for (int i = 0; i < this.dimensionX; i++) {
+			if (i == column) continue;
+			for (int j = 0; j < this.dimensionY; j++) {
+				if (j == row) continue;
+				matrix[i < column? i : i - 1][j < row? j : j - 1] = this.matrix[i][j];
+			}
+		}
+		return construct(matrix);
+	}
+
+	public Vectord asVector() {
+		if (dimensionX != 1 && dimensionY != 1) throw new UnsupportedOperationException("Cannot get Matrix of non vector dimensions as vector.");
+		if (dimensionX == 1) return getColumn(0);
+		else return getRow(0);
+	}
+
+	public T transpose() {
+		double[][] transposed = new double[this.dimensionY][this.dimensionX];
+		for (int i = 0; i < dimensionX; i++) {
+			for (int j = 0; j < dimensionY; j++) {
+				transposed[j][i] = matrix[j][i];
+			}
+		}
+		return construct(transposed);
+	}
+
+	public double determinant() {
+		if (dimensionY != dimensionX) throw new UnsupportedOperationException("Cannot get the determinant of matrix with differing dimensions.");
+		return det();
+	}
+
+	// determinant as per https://en.wikipedia.org/wiki/Determinant#Laplace's_expansion_and_the_adjugate_matrix
+	protected double det() {
+		if (dimensionX == 0) return 1;
+		double sum = 0;
+		for (int i = 0; i < dimensionX; i++) {
+			if (i % 2 == 0) {
+				sum += matrix[i][0] * this.dropColumnAndRow(i, 0).det();
+			} else {
+				sum -= matrix[i][0] * this.dropColumnAndRow(i, 0).det();
+			}
+		}
+		return sum;
+	}
+
+	public T product(MatrixdImpl<?> matrix) {
+		if (dimensionX != matrix.dimensionY) throw new UnsupportedOperationException("Cannot perform matrix product on matrices of non matching row length and column length");
+		double[][] result = new double[matrix.dimensionX][dimensionY];
+		for (int i = 0; i < matrix.dimensionX; i++) {
+			for (int j = 0; j < dimensionY; j++) {
+				result[i][j] = matrix.getColumn(i).dot(getRow(j));
+			}
+		}
+		return construct(result);
+	}
+
+	public Matrixd universalProduct(MatrixdImpl<?> matrix) {
+		if (dimensionX != matrix.dimensionY) throw new UnsupportedOperationException("Cannot perform matrix product on matrices of non matching row length and column length");
+		double[][] result = new double[matrix.dimensionX][dimensionY];
+		for (int i = 0; i < matrix.dimensionX; i++) {
+			for (int j = 0; j < dimensionY; j++) {
+				result[i][j] = matrix.getColumn(i).dot(getRow(j));
+			}
+		}
+		return new Matrixd(result);
+	}
+
+	public Vectord product(Vectord vector) {
+		MatrixdImpl<?> matrix = new Matrixd(vector);
+		return universalProduct(matrix).asVector();
+	}
+
+	@Override
+	public String toString() {
+		StringBuilder stringBuilder = new StringBuilder();
+		stringBuilder.append("[");
+		for (int j = 0; j < matrix[0].length; j++) {
+			stringBuilder.append("[");
+			for (int i = 0; i < matrix.length; i++) {
+				stringBuilder.append(matrix[i][j]);
+				if (i < matrix.length - 1)
+					stringBuilder.append(",");
+			}
+			stringBuilder.append("]");
+			if (j < matrix[0].length - 1)
+				stringBuilder.append(",\n");
+		}
+		stringBuilder.append("]");
+		return stringBuilder.toString();
+	}
+
+	@Override
+	public boolean equals(Object o) {
+		if (!(o instanceof MatrixdImpl)) return false;
+		MatrixdImpl<?> matrixd = (MatrixdImpl<?>) o;
+		if (matrixd.dimensionX != dimensionX || matrixd.dimensionY != dimensionY) return false;
+
+		for (int i = 0; i < dimensionX; i++) {
+			for (int j = 0; j < dimensionY; j++) {
+				if (matrixd.matrix[i][j] != matrix[i][j]) return false;
+			}
+		}
+		return true;
+	}
+}

src/main/java/org/dimdev/dimdoors/util/math/TransformationMatrix3d.java

@@ -0,0 +1,103 @@
+package org.dimdev.dimdoors.util.math;
+
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
+
+public class TransformationMatrix3d extends TransformationMatrixdImpl<TransformationMatrix3d> {
+	public TransformationMatrix3d(double[][] matrix) {
+		super(matrix);
+	}
+
+	public TransformationMatrix3d(MatrixdImpl<? extends MatrixdImpl<?>> matrix) {
+		super(matrix);
+	}
+
+	public TransformationMatrix3d(Vectord... vectors) {
+		super(vectors);
+	}
+
+	public static TransformationMatrix3d identity() {
+		return new TransformationMatrix3d(Matrixd.identity(4, 4));
+	}
+
+	@Override
+	public TransformationMatrix3d construct(double[][] matrix) {
+		return new TransformationMatrix3d(matrix);
+	}
+
+	@Override
+	public TransformationMatrix3d construct(MatrixdImpl<? extends MatrixdImpl<?>> matrixd) {
+		return new TransformationMatrix3d(matrixd);
+	}
+
+	@Override
+	public TransformationMatrix3d construct(Vectord... vectors) {
+		return new TransformationMatrix3d(vectors);
+	}
+
+	public Vec3d transform(Vec3d vector) {
+		Vectord vec = transform(new Vectord(vector.x, vector.y, vector.z));
+		return new Vec3d(vec.get(0), vec.get(1), vec.get(2));
+	}
+
+	// Should only be called on pure rotation matrices, behaviour undefined otherwise.
+	public EulerAngle transform(EulerAngle angle) {
+		TransformationMatrix3d rotator = TransformationMatrix3d.builder().rotate(angle).build();
+		// angle vector representation
+		Vec3d direction = rotator.transform(new Vec3d(0, 0, 1));
+		Vec3d upwards = rotator.transform(new Vec3d(0, 1, 0));
+
+		direction = transform(direction);
+		upwards = transform(upwards);
+
+		return MathUtil.eulerAngle(direction, upwards);
+	}
+
+	public static TransformationMatrix3dBuilder builder() {
+		return new TransformationMatrix3dBuilder(identity());
+	}
+
+	public static class TransformationMatrix3dBuilder extends TransformationMatrixdBuilderImpl<TransformationMatrix3dBuilder, TransformationMatrix3d> {
+		protected TransformationMatrix3dBuilder(TransformationMatrix3d instance) {
+			super(3, instance);
+		}
+
+		@Override
+		public TransformationMatrix3dBuilder getSelf() {
+			return this;
+		}
+
+		public TransformationMatrix3dBuilder translate(Vec3d vector) {
+			return translate(new Vectord(vector.x, vector.y, vector.z));
+		}
+
+		public TransformationMatrix3dBuilder inverseTranslate(Vec3d vector) {
+			return translate(new Vectord(-vector.x, -vector.y, -vector.z));
+		}
+
+
+		public TransformationMatrix3dBuilder rotateX(double angle) {
+			return rotateAroundBasePlane(angle, 1, 2);
+		}
+
+		public TransformationMatrix3dBuilder rotateY(double angle) {
+			return rotateAroundBasePlane(angle, 2, 0);
+		}
+
+		public TransformationMatrix3dBuilder rotateZ(double angle) {
+			return rotateAroundBasePlane(angle, 0, 1);
+		}
+
+		public TransformationMatrix3dBuilder rotate(EulerAngle angle) {
+			return this.rotateZ(Math.toRadians(angle.getRoll())) // roll
+					.rotateX(Math.toRadians(angle.getPitch())) // pitch
+					.rotateY(Math.toRadians(-angle.getYaw())); // yaw
+		}
+
+		public TransformationMatrix3dBuilder inverseRotate(EulerAngle angle) {
+			return this.rotateZ(-Math.toRadians(angle.getRoll())) // roll
+					.rotateX(-Math.toRadians(angle.getPitch())) // pitch
+					.rotateY(-Math.toRadians(-angle.getYaw())); // yaw
+		}
+	}
+}

src/main/java/org/dimdev/dimdoors/util/math/TransformationMatrixd.java

@@ -0,0 +1,50 @@
+package org.dimdev.dimdoors.util.math;
+
+public class TransformationMatrixd extends TransformationMatrixdImpl<TransformationMatrixd> {
+	public TransformationMatrixd(double[][] matrix) {
+		super(matrix);
+	}
+
+	public TransformationMatrixd(MatrixdImpl<? extends MatrixdImpl<?>> matrix) {
+		super(matrix);
+	}
+
+	public TransformationMatrixd(Vectord... vectors) {
+		super(vectors);
+	}
+
+
+	public static TransformationMatrixd identity(int base) {
+		return new TransformationMatrixd(Matrixd.identity(base + 1, base + 1));
+	}
+
+	public static TransformationMatrixdBuilder builder(int base) {
+		return new TransformationMatrixdBuilder(base, identity(base));
+	}
+
+	@Override
+	public TransformationMatrixd construct(double[][] matrix) {
+		return new TransformationMatrixd(matrix);
+	}
+
+	@Override
+	public TransformationMatrixd construct(MatrixdImpl<? extends MatrixdImpl<?>> matrixd) {
+		return new TransformationMatrixd(matrixd);
+	}
+
+	@Override
+	public TransformationMatrixd construct(Vectord... vectors) {
+		return new TransformationMatrixd(vectors);
+	}
+
+	public static class TransformationMatrixdBuilder extends TransformationMatrixdBuilderImpl<TransformationMatrixdBuilder, TransformationMatrixd> {
+		protected TransformationMatrixdBuilder(int base, TransformationMatrixd instance) {
+			super(base, instance);
+		}
+
+		@Override
+		public TransformationMatrixdBuilder getSelf() {
+			return this;
+		}
+	}
+}

src/main/java/org/dimdev/dimdoors/util/math/TransformationMatrixdImpl.java

@@ -0,0 +1,88 @@
+package org.dimdev.dimdoors.util.math;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public abstract class TransformationMatrixdImpl<T extends MatrixdImpl<T>> extends MatrixdImpl<T> {
+	public TransformationMatrixdImpl(double[][] matrix) {
+		super(matrix);
+		if (getDimensionX() != getDimensionY()) {
+			throw new UnsupportedOperationException("Cannot create TransformationMatrixd from non square 2D array.");
+		}
+	}
+
+	public TransformationMatrixdImpl(MatrixdImpl<?> matrix) {
+		super(matrix);
+		if (getDimensionX() != getDimensionY()) {
+			throw new UnsupportedOperationException("Cannot create TransformationMatrixd from non square matrix.");
+		}
+	}
+
+	public TransformationMatrixdImpl(Vectord... vectors) {
+		super(vectors);
+		if (getDimensionX() != getDimensionY()) {
+			throw new UnsupportedOperationException("Cannot create TransformationMatrixd from non square vector array.");
+		}
+	}
+
+	public int getBase() {
+		return getDimensionX() - 1;
+	}
+
+	public Vectord transform(Vectord vector) {
+		if (vector.size() != getBase()) throw new UnsupportedOperationException("Cannot transform vector of non matching base");
+
+		return product(vector.append(1)).drop(vector.size());
+	}
+
+	public static abstract class TransformationMatrixdBuilderImpl<V extends TransformationMatrixdBuilderImpl<V, U>, U extends TransformationMatrixdImpl<U>> {
+		private final int base;
+		private final U instance;
+		private final List<TransformationMatrixdImpl<?>> transformers = new ArrayList<>();
+		private final List<TransformationMatrixdImpl<?>> reverseTransformers = new ArrayList<>();
+
+		protected TransformationMatrixdBuilderImpl(int base, U instance) {
+			this.base = base;
+			this.instance = instance;
+		}
+
+		public abstract V getSelf();
+
+		public U build() {
+			TransformationMatrixd transformer = TransformationMatrixd.identity(base);
+			for (TransformationMatrixdImpl<?> transformationMatrix : transformers) {
+				transformer = transformer.product(transformationMatrix);
+			}
+			return instance.construct(transformer);
+		}
+
+		public U buildReverse() {
+			TransformationMatrixd transformer = TransformationMatrixd.identity(base);
+			for (TransformationMatrixdImpl<?> transformationMatrix : reverseTransformers) {
+				transformer = transformer.product(transformationMatrix);
+			}
+			return instance.construct(transformer);
+		}
+
+		public V translate(Vectord translation) {
+			TransformationMatrixdImpl<?> transformer = TransformationMatrixd.identity(base).setColumn(base, translation.append(1));
+			transformers.add(0, transformer);
+			TransformationMatrixdImpl<?> reverseTransformer = TransformationMatrixd.identity(base).setColumn(base, translation.invert().append(1));
+			reverseTransformers.add(reverseTransformer);
+			return getSelf();
+		}
+
+		public V rotateAroundBasePlane(double angle, int planeBaseVectorIndex1, int planeBaseVectorIndex2) {
+			Vectord column1 = new Vectord(base + 1).set(planeBaseVectorIndex1, Math.cos(angle)).set(planeBaseVectorIndex2, Math.sin(angle));
+			Vectord column2 = new Vectord(base + 1).set(planeBaseVectorIndex1, -Math.sin(angle)).set(planeBaseVectorIndex2, Math.cos(angle));
+			TransformationMatrixdImpl<?> transformer = TransformationMatrixd.identity(base).setColumn(planeBaseVectorIndex1, column1).setColumn(planeBaseVectorIndex2, column2);
+			transformers.add(0, transformer);
+
+			column1 = new Vectord(base + 1).set(planeBaseVectorIndex1, Math.cos(angle)).set(planeBaseVectorIndex2, -Math.sin(angle));
+			column2 = new Vectord(base + 1).set(planeBaseVectorIndex1, Math.sin(angle)).set(planeBaseVectorIndex2, Math.cos(angle));
+			TransformationMatrixdImpl<?> reverseTransformer = TransformationMatrixd.identity(base).setColumn(planeBaseVectorIndex1, column1).setColumn(planeBaseVectorIndex2, column2);
+			reverseTransformers.add(reverseTransformer);
+			return getSelf();
+		}
+	}
+}

src/main/java/org/dimdev/dimdoors/util/math/Vectord.java

@@ -0,0 +1,91 @@
+package org.dimdev.dimdoors.util.math;
+
+public class Vectord {
+	private final double[] vec;
+
+	public Vectord(int size) {
+		vec = new double[size];
+	}
+
+	public Vectord(double... vec) {
+		this.vec = vec;
+	}
+
+	protected double[] getVec() {
+		return vec;
+	}
+
+	public double get(int index) {
+		return vec[index];
+	}
+
+	public Vectord set(int index, double value) {
+		double[] vec = this.vec;
+		vec[index] = value;
+		return new Vectord(vec);
+	}
+
+	public Vectord drop(int index) {
+		double[] vec = new double[size() - 1];
+		for (int i = 0; i < size(); i++) {
+			if (i == index) continue;
+			vec[i < index? i : i - 1] = this.vec[i];
+		}
+		return new Vectord(vec);
+	}
+
+	public Vectord append(double value) {
+		double[] extended = new double[size() + 1];
+		for (int i = 0; i < size(); i++) {
+			extended[i] = vec[i];
+		}
+		extended[size()] = value;
+		return new Vectord(extended);
+	}
+
+	public int size() {
+		return vec.length;
+	}
+
+	public Vectord invert() {
+		return mult(-1);
+	}
+
+	public Vectord mult(double value) {
+		double[] vec = this.vec;
+		for (int i = 0; i < vec.length; i++) {
+			vec[i] *= value;
+		}
+		return new Vectord(vec);
+	}
+
+	public double dot(Vectord vector) {
+		if (vector.size() != this.size()) throw new UnsupportedOperationException("Cannot apply dot product to vectors of different size.");
+		double sum = 0;
+		for (int i = 0; i < this.size(); i++) {
+			sum += this.get(i) * vector.get(i);
+		}
+		return sum;
+	}
+
+	// TODO: cross product as per https://en.wikipedia.org/wiki/Cross_product#Multilinear_algebra
+	public Vectord cross(Vectord... vectors) {
+		if (vectors.length != size() - 2) throw new UnsupportedOperationException("Cannot perform " + size() +"D vector cross product with " + (vectors.length + 1) + " vectors.");
+		Vectord[] allVectors = new Vectord[vectors.length + 1];
+		allVectors[0] = this;
+		for (int i = 0; i < vectors.length; i++) {
+			allVectors[i + 1] = vectors[i];
+		}
+		Matrixd matrix = new Matrixd(allVectors).transpose();
+
+		double[] vector = new double[size()];
+		for (int i = 0; i < size(); i++) {
+			if ((i + size()) % 2 == 0) {
+				vector[i] = matrix.dropColumn(i).determinant();
+			} else {
+				vector[i] = -matrix.dropColumn(i).determinant();
+			}
+		}
+		return new Vectord(vector);
+	}
+}

src/main/resources/assets/dimdoors/models/item/rift_configuration_tool.json

@@ -1,5 +1,5 @@
 {
-  "parent": "item/generated",
+  "parent": "item/handheld",
   "textures": {
     "layer0": "dimdoors:item/rift_configuration_tool"
   }

src/main/schematics/org/dimdev/dimdoors/util/schematic/SchematicConverter.java

@@ -10,10 +10,6 @@ public class SchematicConverter {
 	private static final String[] BRICK_VARIANTS = new String[]{"stone_brick", "nether_brick"};
 
 	public static String updateId(String id) {
-
-		if (id.equals("minecraft:redstone_torch[facing=north]")) {
-			System.out.println();
-		}
 		id = CONVERSIONS.getOrDefault(id, id);
 		return id;
 	}

src/main/schematics/org/dimdev/dimdoors/util/schematic/v2/SchematicBlockPalette.java

@@ -32,16 +32,12 @@ public class SchematicBlockPalette {
 				Block block = Objects.requireNonNull(Registry.BLOCK.get(new Identifier(string.substring(0, string.indexOf("[")))));
 				BlockState state = block.getDefaultState();
 
-				System.out.println(state);
-
 				String[] stateArray = string.substring(string.indexOf("[") + 1, string.length() - 1).split(",");
 				for (String stateString : stateArray) {
 					Property<?> property = Objects.requireNonNull(block.getStateManager().getProperty(stateString.split("=")[0]));
 					state = process(property, stateString.split("=")[1], state);
 				}
 
-				System.out.println(state);
-
 				return DataResult.success(state);
 			}
 		}

src/test/java/org/dimdev/dimdoors/util/math/MathUtilTest.java

@@ -0,0 +1,45 @@
+package org.dimdev.dimdoors.util.math;
+
+import net.minecraft.util.math.Direction;
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
+import org.dimdev.test.TestUtil;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+class MathUtilTest {
+
+	@Test
+	void eulerAngle() {
+		EulerAngle expected = new EulerAngle(0, 0, 0);
+
+		Vec3d direction = new Vec3d(0, 0, 1);
+		Vec3d upwards = new Vec3d(0, 1, 0);
+		EulerAngle angle = MathUtil.eulerAngle(direction, upwards);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		expected = new EulerAngle(-90, 0, 0);
+		direction = new Vec3d(0, 1, 0);
+		upwards = new Vec3d(0, 0, -1);
+		angle = MathUtil.eulerAngle(direction, upwards);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		expected = new EulerAngle(0, -45, 0);
+		direction = new Vec3d(Math.cos(Math.PI / 2), 0, Math.cos(Math.PI / 2));
+		upwards = new Vec3d(0, 1, 0);
+		angle = MathUtil.eulerAngle(direction, upwards);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+	}
+
+	@Test
+	void directionEulerAngle() {
+		for (Direction direction : Direction.values()) {
+			EulerAngle expected = MathUtil.directionEulerAngle(direction);
+			Vec3d dir = Vec3d.of(direction.getVector());
+			EulerAngle angle = new EulerAngle(MathUtil.pitch(dir), MathUtil.yaw(dir), 0);
+
+			assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+		}
+	}
+}

src/test/java/org/dimdev/dimdoors/util/math/TransformationMatrix3dTest.java

@@ -0,0 +1,129 @@
+package org.dimdev.dimdoors.util.math;
+
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
+import org.dimdev.test.TestUtil;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+class TransformationMatrix3dTest {
+
+	@Test
+	void identity() {
+		double[][] matrix = new double[4][4];
+		matrix[0] = new double[]{1, 0, 0, 0};
+		matrix[1] = new double[]{0, 1, 0, 0};
+		matrix[2] = new double[]{0, 0, 1, 0};
+		matrix[3] = new double[]{0, 0, 0, 1};
+		TransformationMatrix3d identity = new TransformationMatrix3d(matrix);
+		assertEquals(identity, TransformationMatrix3d.identity());
+
+		matrix[3] = new double[]{1, 0, 0, 1};
+		TransformationMatrix3d matrix3d = new TransformationMatrix3d(matrix);
+		assertNotEquals(matrix3d, TransformationMatrix3d.identity());
+	}
+
+	@Test
+	void transformVec3d() {
+		// rotate around
+		Vec3d vector = new Vec3d(1, 0, 0);
+		Vec3d expected;
+		TransformationMatrix3d rotate90DegreesY = TransformationMatrix3d.builder().rotateY(Math.PI / 2).build();
+
+		vector = rotate90DegreesY.transform(vector);
+		expected = new Vec3d(0, 0, -1);
+		assertTrue(TestUtil.closeEnough(expected, vector), TestUtil.expectedActual(expected, vector));
+
+		vector = rotate90DegreesY.transform(vector);
+		expected = new Vec3d(-1, 0, 0);
+		assertTrue(TestUtil.closeEnough(expected, vector), TestUtil.expectedActual(expected, vector));
+
+		vector = rotate90DegreesY.transform(vector);
+		expected = new Vec3d(0, 0, 1);
+		assertTrue(TestUtil.closeEnough(expected, vector), TestUtil.expectedActual(expected, vector));
+
+		vector = rotate90DegreesY.transform(vector);
+		expected = new Vec3d(1, 0, 0);
+		assertTrue(TestUtil.closeEnough(expected, vector), TestUtil.expectedActual(expected, vector));
+
+		TransformationMatrix3d rotate45DegreesY = TransformationMatrix3d.builder().rotateY(Math.PI / 4).build();
+
+		vector = rotate45DegreesY.transform(vector);
+		expected = new Vec3d(Math.cos(Math.PI/4), 0, -Math.sin(Math.PI/4));
+		assertTrue(TestUtil.closeEnough(expected, vector), TestUtil.expectedActual(expected, vector));
+
+		double random = Math.random()*2*Math.PI;
+		expected = new Vec3d(Math.cos(random), 0, -Math.sin(random));
+		TransformationMatrix3d.TransformationMatrix3dBuilder builder = TransformationMatrix3d.builder()
+				.rotate(new EulerAngle((((float) Math.random()) - 0.5F) * 180, (((float) Math.random()) - 0.5F) * 360, (((float) Math.random()) - 0.5F) * 360))
+				.translate(new Vec3d(Math.random()*100, Math.random()*100, Math.random()*100))
+				.rotate(new EulerAngle((((float) Math.random()) - 0.5F) * 180, (((float) Math.random()) - 0.5F) * 360, (((float) Math.random()) - 0.5F) * 360))
+				.translate(new Vec3d(Math.random()*100, Math.random()*100, Math.random()*100));
+
+		vector = builder.buildReverse().transform(builder.build().transform(expected));
+		assertTrue(TestUtil.closeEnough(expected, vector), TestUtil.expectedActual(expected, vector));
+
+	}
+
+	@Test
+	void transformEulerAngle() {
+		EulerAngle expected;
+		EulerAngle angle;
+
+		TransformationMatrix3d identity = TransformationMatrix3d.identity();
+
+		expected = new EulerAngle(0, 0, 0);
+		angle = identity.transform(expected);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		expected = new EulerAngle(90, 0, 0);
+		angle = identity.transform(expected);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		expected = new EulerAngle(0, 90, 0);
+		angle = identity.transform(expected);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		expected = new EulerAngle(0, 0, 90);
+		angle = identity.transform(expected);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		expected = new EulerAngle(90, 90, 90);
+		angle = identity.transform(expected);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		// randomize EulerAngle
+		expected = new EulerAngle((((float) Math.random()) - 0.5F) * 180, (((float) Math.random()) - 0.5F) * 360, (((float) Math.random()) - 0.5F) * 360);
+
+		angle = identity.transform(expected);
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		angle = expected;
+		TransformationMatrix3d rotate90DegreesY = TransformationMatrix3d.builder().rotateY(Math.PI / 2).build();
+		for (int i = 0; i < 4; i++) {
+			angle = rotate90DegreesY.transform(angle);
+		}
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		angle = expected;
+		TransformationMatrix3d rotate90DegreesX = TransformationMatrix3d.builder().rotateX(Math.PI / 2).build();
+		for (int i = 0; i < 4; i++) {
+			angle = rotate90DegreesX.transform(angle);
+		}
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+
+		angle = expected;
+		TransformationMatrix3d rotate90DegreesZ = TransformationMatrix3d.builder().rotateZ(Math.PI / 2).build();
+		for (int i = 0; i < 4; i++) {
+			angle = rotate90DegreesZ.transform(angle);
+		}
+		assertTrue(TestUtil.closeEnough(expected, angle), TestUtil.expectedActual(TestUtil.toString(expected), TestUtil.toString(angle)));
+	}
+
+	@Test
+	void product() {
+		// compare I and I^2
+		assertEquals(TransformationMatrix3d.identity(), TransformationMatrix3d.identity().product(TransformationMatrix3d.identity()));
+	}
+}

src/test/java/org/dimdev/test/TestUtil.java

@@ -0,0 +1,48 @@
+package org.dimdev.test;
+
+import net.minecraft.util.math.EulerAngle;
+import net.minecraft.util.math.Vec3d;
+import org.dimdev.dimdoors.util.math.MatrixdImpl;
+
+import java.util.function.Supplier;
+
+public class TestUtil {
+	public static Supplier<String> expectedActual(Object expected,Object actual) {
+		return () -> "\nexpected:\n" + expected + "\nactual:\n" + actual + "\n";
+	}
+
+	public static boolean closeEnough(Vec3d expected, Vec3d actual) {
+		return expected.squaredDistanceTo(actual) <= expected.lengthSquared() * 1E-10;
+	}
+
+	public static boolean closeEnough(MatrixdImpl<?> expected, MatrixdImpl<?> actual) {
+		if (expected.getDimensionX() != actual.getDimensionX() || expected.getDimensionY() != actual.getDimensionY()) return false;
+
+		for (int i = 0; i < expected.getDimensionX(); i++) {
+			for (int j = 0; j < expected.getDimensionY(); j++) {
+				double entry1 = expected.get(i, j);
+				double entry2 = actual.get(i, j);
+				if (entry1 == entry2) continue;
+				if (entry1 != 0 && entry2 != 0) {
+					double div = entry1/entry2;
+					if (0.991 <= div && div <= 0.001) continue;
+				}
+				if (Math.abs(entry1 - entry2) <= 1E-10) continue;
+				return false;
+			}
+		}
+		return true;
+	}
+
+	public static boolean closeEnough(EulerAngle expected, EulerAngle actual) {
+		float yawDiff = Math.abs(expected.getYaw() - actual.getYaw());
+		float pitchDiff = Math.abs(expected.getPitch() - actual.getPitch());
+		float rollDiff = Math.abs(expected.getRoll() - actual.getRoll());
+
+		return yawDiff <= 1 && pitchDiff <= 1 && rollDiff <= 1;
+	}
+
+	public static String toString(EulerAngle angle) {
+		return "{yaw: " + angle.getYaw() + "; pitch: " + angle.getPitch() + "; roll: " + angle.getRoll() + "}";
+	}
+}