Added Object fields to StatefulObjects and added some utility methods

- Added ObjectStateField
- Added WorldGenericContextRO.findClosestEntity, .forEachEntity
- Added VectorUtil.lookAt, .distance{,Sq}

src/main/java/ru/windcorp/progressia/common/state/AbstractStatefulObjectLayout.java

@@ -29,6 +29,20 @@ public abstract class AbstractStatefulObjectLayout
 		super(objectId);
 	}
 	
+	@Override
+	public StateStorage createStorage() {
+		StateStorage storage = instantiateStorage();
+		
+		int fieldCount = getFieldCount();
+		for (int i = 0; i < fieldCount; ++i) {
+			getField(i).setDefault(storage);
+		}
+		
+		return storage;
+	}
+	
+	protected abstract StateStorage instantiateStorage();
+
 	protected abstract int getFieldCount();
 
 	protected abstract StateField getField(int fieldIndex);

src/main/java/ru/windcorp/progressia/common/state/Encodable.java

@@ -0,0 +1,64 @@
+/*
+ * Progressia
+ * Copyright (C)  2020-2021  Wind Corporation and contributors
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+package ru.windcorp.progressia.common.state;
+
+import java.io.DataInput;
+import java.io.DataOutput;
+import java.io.IOException;
+
+public interface Encodable {
+
+	/**
+	 * Sets the state of this object according to the binary representation read
+	 * from {@code input}. If {@code context == COMMS}, the state of local
+	 * fields is unspecified after this operation.
+	 * 
+	 * @param input   a {@link DataInput} that a state can be read from
+	 * @param context the context
+	 * @throws IOException if the state is encoded poorly or an error occurs
+	 *                     in {@code input}
+	 */
+	void read(DataInput input, IOContext context) throws IOException;
+
+	/**
+	 * Writes the binary representation of the state of this object to the
+	 * {@code output}.
+	 * 
+	 * @param output  a {@link DataOutput} that a state can be written to
+	 * @param context the context
+	 * @throws IOException if an error occurs in {@code output}
+	 */
+	void write(DataOutput output, IOContext context) throws IOException;
+
+	/**
+	 * Turns {@code destination} into a deep copy of this object.
+	 * <p>
+	 * Changes the provided object so that:
+	 * <ul>
+	 * <li>the provided object equals this object; and</li>
+	 * <li>the provided object is independent of this object, meaning no change
+	 * to {@code destination} can affect this object.</li>
+	 * </ul>
+	 * 
+	 * @param destination the object to copy this object into. Runtime class
+	 *                    must match this class
+	 * @return {@code destination}
+	 */
+	void copy(Encodable destination);
+
+}

src/main/java/ru/windcorp/progressia/common/state/HashMapStateStorage.java

@@ -19,11 +19,14 @@
 package ru.windcorp.progressia.common.state;
 
 import gnu.trove.map.TIntIntMap;
+import gnu.trove.map.TIntObjectMap;
 import gnu.trove.map.hash.TIntIntHashMap;
+import gnu.trove.map.hash.TIntObjectHashMap;
 
 public class HashMapStateStorage extends StateStorage {
 
 	private final TIntIntMap ints = new TIntIntHashMap();
+	private final TIntObjectMap<Object> objects = new TIntObjectHashMap<>();
 
 	@Override
 	public int getInt(int index) {
@@ -35,4 +38,14 @@ public class HashMapStateStorage extends StateStorage {
 		ints.put(index, value);
 	}
 	
+	@Override
+	public Object getObject(int index) {
+		return objects.get(index);
+	}
+	
+	@Override
+	public void setObject(int index, Object object) {
+		objects.put(index, object);
+	}
+
 }

src/main/java/ru/windcorp/progressia/common/state/InspectingStatefulObjectLayout.java

@@ -20,6 +20,9 @@ package ru.windcorp.progressia.common.state;
 
 import java.util.ArrayList;
 import java.util.List;
+import java.util.function.Supplier;
+
+import ru.windcorp.progressia.common.state.codec.ObjectCodec;
 
 public class InspectingStatefulObjectLayout
 	extends AbstractStatefulObjectLayout {
@@ -33,7 +36,7 @@ public class InspectingStatefulObjectLayout
 	}
 
 	@Override
-	public StateStorage createStorage() {
+	public StateStorage instantiateStorage() {
 		return new HashMapStateStorage();
 	}
 
@@ -82,6 +85,31 @@ public class InspectingStatefulObjectLayout
 
 		}
 		
+		private class Obj<T> implements StateFieldBuilder.Obj<T> {
+
+			private final ObjectCodec<T> codec;
+			private final Supplier<T> defaultValue;
+
+			public Obj(ObjectCodec<T> codec, Supplier<T> defaultValue) {
+				this.codec = codec;
+				this.defaultValue = defaultValue;
+			}
+
+			@Override
+			public ObjectStateField<T> build() {
+				return registerField(
+					new ObjectStateField<T>(
+						id,
+						isLocal,
+						fieldIndexCounters.getObjectsThenIncrement(),
+						codec,
+						defaultValue
+					)
+				);
+			}
+
+		}
+
 		private final String id;
 
 		private boolean isLocal = true;
@@ -95,6 +123,11 @@ public class InspectingStatefulObjectLayout
 			return new Int();
 		}
 		
+		@Override
+		public <T> Obj<T> of(ObjectCodec<T> codec, Supplier<T> defaultValue) {
+			return new Obj<T>(codec, defaultValue);
+		}
+
 		@Override
 		public StateFieldBuilder setLocal(boolean isLocal) {
 			this.isLocal = isLocal;

src/main/java/ru/windcorp/progressia/common/state/IntStateField.java

@@ -79,4 +79,9 @@ public class IntStateField extends StateField {
 		return get(a) == get(b);
 	}
 	
+	@Override
+	public void setDefault(StateStorage storage) {
+		storage.setInt(getIndex(), 0);
+	}
+
 }

src/main/java/ru/windcorp/progressia/common/state/ObjectStateField.java

@@ -0,0 +1,107 @@
+/*
+ * Progressia
+ * Copyright (C)  2020-2021  Wind Corporation and contributors
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+ 
+package ru.windcorp.progressia.common.state;
+
+import java.io.DataInput;
+import java.io.DataOutput;
+import java.io.IOException;
+import java.util.function.Supplier;
+
+import ru.windcorp.progressia.common.state.codec.ObjectCodec;
+
+public class ObjectStateField<T> extends StateField {
+	
+	private final ObjectCodec<T> codec;
+	private final Supplier<T> defaultValue;
+
+	public ObjectStateField(
+		String id,
+		boolean isLocal,
+		int index,
+		ObjectCodec<T> codec,
+		Supplier<T> defaultValue
+	) {
+		super(id, isLocal, index);
+		
+		this.codec = codec;
+		this.defaultValue = defaultValue;
+	}
+	
+	public ObjectCodec<T> getCodec() {
+		return codec;
+	}
+
+	@SuppressWarnings("unchecked")
+	public T get(StatefulObject object) {
+		return (T) object.getStorage().getObject(getIndex());
+	}
+
+	public void setNow(StatefulObject object, T value) {
+		object.getStorage().setObject(getIndex(), value);
+	}
+
+	public void set(StateChanger changer, T value) {
+		changer.setObject(this, value);
+	}
+
+	@Override
+	public void read(
+		StatefulObject object,
+		DataInput input,
+		IOContext context
+	)
+		throws IOException {
+		
+		T previous = get(object);
+		T result = codec.read(previous, input, context);
+		object.getStorage().setObject(getIndex(), result);
+	}
+
+	@Override
+	public void write(
+		StatefulObject object,
+		DataOutput output,
+		IOContext context
+	)
+		throws IOException {
+		
+		codec.write(object.getStorage().getObject(getIndex()), output, context);
+	}
+
+	@Override
+	public void copy(StatefulObject from, StatefulObject to) {
+		setNow(to, get(from));
+	}
+
+	@Override
+	public int computeHashCode(StatefulObject object) {
+		return codec.computeHashCode(get(object));
+	}
+
+	@Override
+	public boolean areEqual(StatefulObject a, StatefulObject b) {
+		return codec.areEqual(get(a), get(b));
+	}
+	
+	@Override
+	public void setDefault(StateStorage storage) {
+		storage.setObject(getIndex(), defaultValue.get());
+	}
+
+}

src/main/java/ru/windcorp/progressia/common/state/OptimizedStateStorage.java

@@ -21,9 +21,11 @@ package ru.windcorp.progressia.common.state;
 public class OptimizedStateStorage extends StateStorage {
 
 	private final int[] ints;
+	private final Object[] objects;
 
 	public OptimizedStateStorage(PrimitiveCounters sizes) {
 		this.ints = new int[sizes.getInts()];
+		this.objects = new Object[sizes.getObjects()];
 	}
 
 	@Override
@@ -36,4 +38,14 @@ public class OptimizedStateStorage extends StateStorage {
 		ints[index] = value;
 	}
 	
+	@Override
+	public Object getObject(int index) {
+		return objects[index];
+	}
+	
+	@Override
+	public void setObject(int index, Object object) {
+		objects[index] = object;
+	}
+
 }

src/main/java/ru/windcorp/progressia/common/state/OptimizedStatefulObjectLayout.java

@@ -19,9 +19,12 @@
 package ru.windcorp.progressia.common.state;
 
 import java.util.List;
+import java.util.function.Supplier;
 
 import com.google.common.collect.ImmutableList;
 
+import ru.windcorp.progressia.common.state.codec.ObjectCodec;
+
 public class OptimizedStatefulObjectLayout
 	extends AbstractStatefulObjectLayout {
 
@@ -49,7 +52,7 @@ public class OptimizedStatefulObjectLayout
 	}
 
 	@Override
-	public StateStorage createStorage() {
+	public StateStorage instantiateStorage() {
 		return new OptimizedStateStorage(sizes);
 	}
 
@@ -72,6 +75,17 @@ public class OptimizedStatefulObjectLayout
 			};
 		}
 		
+		@Override
+		public <T> Obj<T> of(ObjectCodec<T> codec, Supplier<T> defaultValue) {
+			return new Obj<T>() {
+				@SuppressWarnings("unchecked")
+				@Override
+				public ObjectStateField<T> build() {
+					return (ObjectStateField<T>) result;
+				}
+			};
+		}
+
 		@Override
 		public StateFieldBuilder setLocal(boolean isLocal) {
 			return this;

src/main/java/ru/windcorp/progressia/common/state/PrimitiveCounters.java

@@ -21,12 +21,14 @@ package ru.windcorp.progressia.common.state;
 class PrimitiveCounters {
 
 	private int ints = 0;
+	private int objects = 0;
 
 	public PrimitiveCounters() {
 	}
 
 	public PrimitiveCounters(PrimitiveCounters copyFrom) {
 		this.ints = copyFrom.ints;
+		this.objects = copyFrom.objects;
 	}
 
 	public int getInts() {
@@ -37,4 +39,12 @@ class PrimitiveCounters {
 		return this.ints++;
 	}
 	
+	public int getObjects() {
+		return objects;
+	}
+
+	public int getObjectsThenIncrement() {
+		return this.objects++;
+	}
+
 }

src/main/java/ru/windcorp/progressia/common/state/StateChanger.java

@@ -21,5 +21,6 @@ package ru.windcorp.progressia.common.state;
 public interface StateChanger {
 
 	void setInt(IntStateField field, int value);
+	<T> void setObject(ObjectStateField<T> field, T value);
 
 }

src/main/java/ru/windcorp/progressia/common/state/StateField.java

@@ -67,4 +67,6 @@ public abstract class StateField extends Namespaced {
 
 	public abstract boolean areEqual(StatefulObject a, StatefulObject b);
 	
+	public abstract void setDefault(StateStorage storage);
+
 }

src/main/java/ru/windcorp/progressia/common/state/StateFieldBuilder.java

@@ -18,14 +18,59 @@
  
 package ru.windcorp.progressia.common.state;
 
+import java.util.function.Supplier;
+
+import ru.windcorp.progressia.common.state.codec.ObjectCodec;
+import ru.windcorp.progressia.common.state.codec.ObjectCodecRegistry;
+
 public interface StateFieldBuilder {
 
 	public static interface Int {
 		IntStateField build();
 	}
 	
+	public static interface Obj<T> {
+		ObjectStateField<T> build();
+	}
+
 	Int ofInt();
 	
+	<T> Obj<T> of(ObjectCodec<T> codec, Supplier<T> defaultValue);
+	
+	default <T> Obj<T> of(Class<T> clazz, Supplier<T> defaultValue) {
+		ObjectCodec<T> codec = ObjectCodecRegistry.get(clazz);
+		return of(codec, defaultValue);
+	}
+	
+	default <T> Obj<T> of(ObjectCodec<T> codec, T defaultValue) {
+		return of(codec, (Supplier<T>) () -> codec.copy(defaultValue, null));
+	}
+	
+	default <T> Obj<T> of(Class<T> clazz, T defaultValue) {
+		ObjectCodec<T> codec = ObjectCodecRegistry.get(clazz);
+		return of(codec, (Supplier<T>) () -> codec.copy(defaultValue, null));
+	}
+	
+	default <T> Obj<T> of(ObjectCodec<T> codec) {
+		return of(codec, (Supplier<T>) () -> null);
+	}
+	
+	default <T> Obj<T> of(Class<T> clazz) {
+		ObjectCodec<T> codec = ObjectCodecRegistry.get(clazz);
+		return of(codec, (Supplier<T>) () -> null);
+	}
+	
+	@SuppressWarnings("unchecked")
+	default <T> Obj<T> def(Supplier<T> defaultValue) {
+		Class<T> clazz = (Class<T>) defaultValue.get().getClass();
+		return of(clazz, defaultValue);
+	}
+	
+	@SuppressWarnings("unchecked")
+	default <T> Obj<T> def(T defaultValue) {
+		return of((Class<T>) defaultValue.getClass(), defaultValue);
+	}
+
 	StateFieldBuilder setLocal(boolean isLocal);
 
 	default StateFieldBuilder setLocal() {

src/main/java/ru/windcorp/progressia/common/state/StateStorage.java

@@ -24,4 +24,8 @@ public abstract class StateStorage {
 
 	public abstract void setInt(int index, int value);
 	
+	public abstract Object getObject(int index);
+	
+	public abstract void setObject(int index, Object object);
+
 }

src/main/java/ru/windcorp/progressia/common/state/StatefulObject.java

@@ -52,7 +52,7 @@ import ru.windcorp.progressia.common.util.namespaces.Namespaced;
  * type.</li>
  * </ul>
  */
-public abstract class StatefulObject extends Namespaced {
+public abstract class StatefulObject extends Namespaced implements Encodable {
 
 	private final StatefulObjectLayout layout;
 
@@ -133,6 +133,7 @@ public abstract class StatefulObject extends Namespaced {
 	 * @throws IOException if the state is encoded poorly or an error occurs
 	 *                     in {@code input}
 	 */
+	@Override
 	public void read(DataInput input, IOContext context) throws IOException {
 		getLayout().read(this, input, context);
 	}
@@ -145,6 +146,7 @@ public abstract class StatefulObject extends Namespaced {
 	 * @param context the context
 	 * @throws IOException if an error occurs in {@code output}
 	 */
+	@Override
 	public void write(DataOutput output, IOContext context) throws IOException {
 		getLayout().write(this, output, context);
 	}
@@ -166,7 +168,8 @@ public abstract class StatefulObject extends Namespaced {
 	 * 
 	 * @param destination the object to copy this object into.
 	 */
-	public StatefulObject copy(StatefulObject destination) {
+	@Override
+	public void copy(Encodable destination) {
 		Objects.requireNonNull(destination, "destination");
 
 		if (destination == this) {
@@ -182,8 +185,7 @@ public abstract class StatefulObject extends Namespaced {
 			);
 		}
 
-		getLayout().copy(this, destination);
-		return destination;
+		getLayout().copy(this, (StatefulObject) destination);
 	}
 
 	/**

src/main/java/ru/windcorp/progressia/common/state/codec/EncodableCodec.java

@@ -0,0 +1,52 @@
+/*
+ * Progressia
+ * Copyright (C)  2020-2021  Wind Corporation and contributors
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+package ru.windcorp.progressia.common.state.codec;
+
+import java.io.DataInput;
+import java.io.DataOutput;
+import java.io.IOException;
+import java.util.Objects;
+
+import ru.windcorp.progressia.common.state.Encodable;
+import ru.windcorp.progressia.common.state.IOContext;
+
+public class EncodableCodec extends ReusableObjectCodec<Encodable> {
+
+	public EncodableCodec() {
+		super(Encodable.class);
+	}
+	
+	@Override
+	protected Encodable doRead(Encodable previous, DataInput input, IOContext context) throws IOException {
+		previous.read(input, context);
+		return previous;
+	}
+	
+	@Override
+	protected void doWrite(Encodable obj, DataOutput output, IOContext context) throws IOException {
+		obj.write(output, context);
+	}
+	
+	@Override
+	protected Encodable doCopy(Encodable object, Encodable previous) {
+		Objects.requireNonNull(previous, "previous");
+		object.copy(previous);
+		return previous;
+	}
+
+}

src/main/java/ru/windcorp/progressia/common/state/codec/ImmutableObjectCodec.java

@@ -0,0 +1,43 @@
+/*
+ * Progressia
+ * Copyright (C)  2020-2021  Wind Corporation and contributors
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+package ru.windcorp.progressia.common.state.codec;
+
+import java.io.DataInput;
+import java.io.IOException;
+
+import ru.windcorp.progressia.common.state.IOContext;
+
+public abstract class ImmutableObjectCodec<T> extends ObjectCodec<T> {
+
+	public ImmutableObjectCodec(Class<T> clazz) {
+		super(clazz);
+	}
+	
+	@Override
+	public final T copy(T object, T previous) {
+		return object;
+	}
+	
+	@Override
+	protected final T doRead(T previous, DataInput input, IOContext context) throws IOException {
+		return doRead(input, context);
+	}
+
+	protected abstract T doRead(DataInput input, IOContext context) throws IOException;
+
+}

src/main/java/ru/windcorp/progressia/common/state/codec/ObjectCodec.java

@@ -0,0 +1,73 @@
+/*
+ * Progressia
+ * Copyright (C)  2020-2021  Wind Corporation and contributors
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+package ru.windcorp.progressia.common.state.codec;
+
+import java.io.DataInput;
+import java.io.DataOutput;
+import java.io.IOException;
+import java.util.Objects;
+
+import ru.windcorp.progressia.common.state.IOContext;
+
+public abstract class ObjectCodec<T> {
+
+	private final Class<T> clazz;
+
+	public ObjectCodec(Class<T> clazz) {
+		this.clazz = clazz;
+	}
+
+	public Class<T> getDataType() {
+		return clazz;
+	}
+
+	@SuppressWarnings("unchecked")
+	public final T read(Object previous, DataInput input, IOContext context) throws IOException {
+		assert previous == null || clazz.isInstance(previous)
+			: "Cannot use codec " + this + " on object of type " + previous.getClass();
+		
+		T result = doRead((T) previous, input, context);
+		
+		assert result == null || clazz.isInstance(previous)
+			: "Codec " + this + " read object of type " + previous.getClass();
+		return result;
+	}
+
+	protected abstract T doRead(T previous, DataInput input, IOContext context) throws IOException;
+
+	@SuppressWarnings("unchecked")
+	public final void write(Object value, DataOutput output, IOContext context) throws IOException {
+		assert value == null || clazz.isInstance(value)
+			: "Cannot use codec " + this + " on object of type " + value.getClass();
+		
+		doWrite((T) value, output, context);
+	}
+
+	protected abstract void doWrite(T obj, DataOutput output, IOContext context) throws IOException;
+
+	public abstract T copy(T object, T previous);
+
+	public int computeHashCode(T object) {
+		return Objects.hashCode(object);
+	}
+
+	public boolean areEqual(T a, T b) {
+		return Objects.equals(a, b);
+	}
+
+}

src/main/java/ru/windcorp/progressia/common/state/codec/ObjectCodecRegistry.java

@@ -0,0 +1,49 @@
+/*
+ * Progressia
+ * Copyright (C)  2020-2021  Wind Corporation and contributors
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+package ru.windcorp.progressia.common.state.codec;
+
+import java.util.Collections;
+import java.util.HashMap;
+import java.util.Map;
+
+import ru.windcorp.progressia.common.state.Encodable;
+
+public class ObjectCodecRegistry {
+	
+	private static final Map<Class<?>, ObjectCodec<?>> CODECS = Collections.synchronizedMap(new HashMap<>());
+	private static final EncodableCodec ENCODABLE_FALLBACK = new EncodableCodec();
+	
+	public static void register(ObjectCodec<?> codec) {
+		CODECS.put(codec.getDataType(), codec);
+	}
+	
+	@SuppressWarnings("unchecked")
+	public static <T> ObjectCodec<T> get(Class<T> clazz) {
+		ObjectCodec<?> codec = CODECS.get(clazz);
+		if (codec != null) {
+			return (ObjectCodec<T>) codec;
+		}
+		
+		if (Encodable.class.isAssignableFrom(clazz)) {
+			return (ObjectCodec<T>) ENCODABLE_FALLBACK;
+		}
+		
+		throw new IllegalArgumentException("No codec registered for class " + clazz);
+	}
+
+}

src/main/java/ru/windcorp/progressia/common/state/codec/ReusableObjectCodec.java

@@ -0,0 +1,42 @@
+/*
+ * Progressia
+ * Copyright (C)  2020-2021  Wind Corporation and contributors
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+package ru.windcorp.progressia.common.state.codec;
+
+public abstract class ReusableObjectCodec<T> extends ObjectCodec<T> {
+
+	public ReusableObjectCodec(Class<T> clazz) {
+		super(clazz);
+	}
+	
+	@Override
+	public final T copy(T object, T previous) {
+		if (object == null) {
+			return null;
+		}
+		
+		T result = doCopy(object, previous);
+		
+		assert result != null : "copy() returned null";
+		assert areEqual(object, result) : "copy() does not equal original: " + result + " != " + object;
+		
+		return result;
+	}
+
+	protected abstract T doCopy(T object, T previous);
+
+}

src/main/java/ru/windcorp/progressia/common/util/VectorUtil.java

@@ -199,6 +199,63 @@ public class VectorUtil {
 		return rotateOnly(inOut, mat, inOut);
 	}
 	
+	public static Mat4 lookAt(Mat4 applyTo, Vec3 target, Vec3 up, Mat4 output) {
+		if (output == null) {
+			output = new Mat4();
+		}
+		
+		Mat4 lookAtTransform = Matrices.grab4();
+		
+		// Adapted from Glm.lookAt - we use Z as up
+		
+		// f(normalize(target))
+        float fX = target.x;
+        float fY = target.y;
+        float fZ = target.z;
+        float inverseSqrt = 1f / (float) Math.sqrt(fX * fX + fY * fY + fZ * fZ);
+        fX *= inverseSqrt;
+        fY *= inverseSqrt;
+        fZ *= inverseSqrt;
+        // s(normalize(cross(up, f)))
+        float sX = up.y * fZ - up.z * fY;
+        float sY = up.z * fX - up.x * fZ;
+        float sZ = up.x * fY - up.y * fX;
+        inverseSqrt = 1.0f / (float) Math.sqrt(sX * sX + sY * sY + sZ * sZ);
+        sX *= inverseSqrt;
+        sY *= inverseSqrt;
+        sZ *= inverseSqrt;
+        // u(cross(f, s))
+        float uX = fY * sZ - fZ * sY;
+        float uY = fZ * sX - fX * sZ;
+        float uZ = fX * sY - fY * sX;
+        
+        lookAtTransform.m00 = fX;
+        lookAtTransform.m01 = fY;
+        lookAtTransform.m02 = fZ;
+        lookAtTransform.m03 = 0f;
+        lookAtTransform.m10 = sX;
+        lookAtTransform.m11 = sY;
+        lookAtTransform.m12 = sZ;
+        lookAtTransform.m13 = 0f;
+        lookAtTransform.m20 = uX;
+        lookAtTransform.m21 = uY;
+        lookAtTransform.m22 = uZ;
+        lookAtTransform.m23 = 0f;
+        lookAtTransform.m30 = 0;
+        lookAtTransform.m31 = 0;
+        lookAtTransform.m32 = 0;
+        lookAtTransform.m33 = 1f;
+		
+		applyTo.mul(lookAtTransform, output);
+		Matrices.release(lookAtTransform);
+		
+		return output;
+	}
+	
+	public static Mat4 lookAt(Vec3 center, Vec3 up, Mat4 inOut) {
+		return lookAt(inOut, center, up, inOut);
+	}
+	
 	public static Vec3 rotate(Vec3 in, Vec3 axis, float angle, Vec3 out) {
 		if (out == null) {
 			out = new Vec3();
@@ -270,6 +327,20 @@ public class VectorUtil {
 		return projectOnVector(inOut, vector);
 	}
 	
+	public static float distanceSq(Vec3 a, Vec3 b) {
+		float x = a.x - b.x;
+		float y = a.y - b.y;
+		float z = a.z - b.z;
+		return x * x + y * y + z * z;
+	}
+	
+	public static float distance(Vec3 a, Vec3 b) {
+		float x = a.x - b.x;
+		float y = a.y - b.y;
+		float z = a.z - b.z;
+		return (float) Math.sqrt(x * x + y * y + z * z);
+	}
+
 	public static Vec3 linearCombination(
 		Vec3 va,
 		float ka,

src/main/java/ru/windcorp/progressia/common/world/generic/context/WorldGenericContextRO.java

@@ -19,8 +19,11 @@ package ru.windcorp.progressia.common.world.generic.context;
 
 import java.util.Collection;
 import java.util.function.Consumer;
+import java.util.function.Predicate;
 
+import glm.vec._3.Vec3;
 import glm.vec._3.i.Vec3i;
+import ru.windcorp.progressia.common.util.VectorUtil;
 import ru.windcorp.progressia.common.world.context.Context;
 import ru.windcorp.progressia.common.world.generic.*;
 import ru.windcorp.progressia.common.world.rels.RelFace;
@@ -141,6 +144,42 @@ public interface WorldGenericContextRO<
 	 */
 	E getEntity(long entityId);
 
+	/*
+	 * Convenience methods
+	 */
+
+	default void forEachEntity(Consumer<E> action) {
+		getEntities().forEach(action);
+	}
+
+	default E findClosestEntity(Vec3 location, Predicate<E> filter, float maxDistance) {
+		if (maxDistance <= 0) {
+			return null;
+		}
+
+		E result = getEntities().stream().filter(filter).min((a, b) -> {
+			float aDistance = VectorUtil.distanceSq(location, a.getPosition());
+			float bDistance = VectorUtil.distanceSq(location, b.getPosition());
+			return Float.compare(aDistance, bDistance);
+		}).orElse(null);
+
+		if (result == null) {
+			return null;
+		}
+		if (Float.isInfinite(maxDistance)) {
+			return result;
+		}
+		if (VectorUtil.distanceSq(location, result.getPosition()) > maxDistance * maxDistance) {
+			return null;
+		}
+
+		return result;
+	}
+	
+	default E findClosestEntity(Vec3 location, float maxDistance) {
+		return findClosestEntity(location, e -> true, maxDistance);
+	}
+
 	/*
 	 * Subcontexting
 	 */