diff --git a/src/main/java/eu/crushedpixel/replaymod/holders/Quaternion.java b/src/main/java/eu/crushedpixel/replaymod/holders/Quaternion.java new file mode 100644 index 00000000..ae21f021 --- /dev/null +++ b/src/main/java/eu/crushedpixel/replaymod/holders/Quaternion.java @@ -0,0 +1,18 @@ +package eu.crushedpixel.replaymod.holders; + +import lombok.AllArgsConstructor; +import lombok.Data; +import lombok.NoArgsConstructor; + +@Data +@AllArgsConstructor +@NoArgsConstructor +public class Quaternion { + + private double w, x, y, z; + + public double dotProduct(Quaternion other) { + return (this.getX() * other.getX() + this.getY() * other.getY() + + this.getZ() * other.getZ() + this.getW() * other.getW()); + } +} diff --git a/src/main/java/eu/crushedpixel/replaymod/interpolation/LerpInterpolation.java b/src/main/java/eu/crushedpixel/replaymod/interpolation/LerpInterpolation.java new file mode 100644 index 00000000..2c2b19f7 --- /dev/null +++ b/src/main/java/eu/crushedpixel/replaymod/interpolation/LerpInterpolation.java @@ -0,0 +1,58 @@ +package eu.crushedpixel.replaymod.interpolation; + +import eu.crushedpixel.replaymod.holders.AdvancedPosition; +import eu.crushedpixel.replaymod.holders.Quaternion; +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; + +import java.util.ArrayList; +import java.util.List; + +public class LerpInterpolation extends GenericLinearInterpolation { + + private List quaternions = new ArrayList(); + + @Override + public void prepare() { + for(AdvancedPosition position : points) { + Rotation rot = new Rotation(RotationOrder.XYZ, + position.getYaw(), position.getPitch(), position.getRoll()); + quaternions.add(new Quaternion(rot.getQ0(), rot.getQ1(), rot.getQ2(), rot.getQ3())); + } + super.prepare(); + } + + @Override + public void applyPoint(float position, AdvancedPosition toEdit) { + super.applyPoint(position, toEdit); + + Quaternion quaternion = new Quaternion(); + + //first, get previous and next Quaternion for given position + float relative = position * (quaternions.size()-1); + int previousIndex = (int)Math.floor(relative); + int nextIndex = (int)Math.ceil(relative); + float percentage = relative - previousIndex; + + Quaternion previous = quaternions.get(previousIndex); + Quaternion next = quaternions.get(nextIndex); + + //interpolate between these Quaternions using the Lerp Algorithm + + double c1 = 1.0f - percentage; + double c2 = percentage; + + quaternion.setX(c1 * previous.getX() + c2 * next.getX()); + quaternion.setY(c1 * previous.getY() + c2 * next.getY()); + quaternion.setZ(c1 * previous.getZ() + c2 * next.getZ()); + quaternion.setW(c1 * previous.getW() + c2 * next.getW()); + + Rotation rotation = new Rotation(quaternion.getW(), quaternion.getX(), + quaternion.getY(), quaternion.getZ(), false); + + double[] angles = rotation.getAngles(RotationOrder.XYZ); + toEdit.setYaw(angles[0]); + toEdit.setPitch(angles[1]); + toEdit.setRoll(angles[2]); + } +} diff --git a/src/main/java/eu/crushedpixel/replaymod/interpolation/SlerpInterpolation.java b/src/main/java/eu/crushedpixel/replaymod/interpolation/SlerpInterpolation.java new file mode 100644 index 00000000..360a7e56 --- /dev/null +++ b/src/main/java/eu/crushedpixel/replaymod/interpolation/SlerpInterpolation.java @@ -0,0 +1,70 @@ +package eu.crushedpixel.replaymod.interpolation; + +import eu.crushedpixel.replaymod.holders.AdvancedPosition; +import eu.crushedpixel.replaymod.holders.Quaternion; +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; + +import java.util.ArrayList; +import java.util.List; + +public class SlerpInterpolation extends GenericSplineInterpolation { + + private List quaternions = new ArrayList(); + + @Override + public void prepare() { + for(AdvancedPosition position : points) { + Rotation rot = new Rotation(RotationOrder.XYZ, + position.getYaw(), position.getPitch(), position.getRoll()); + quaternions.add(new Quaternion(rot.getQ0(), rot.getQ1(), rot.getQ2(), rot.getQ3())); + } + super.prepare(); + } + + @Override + public void applyPoint(float position, AdvancedPosition toEdit) { + super.applyPoint(position, toEdit); + + Quaternion quaternion = new Quaternion(); + + //first, get previous and next Quaternion for given position + float relative = position * (quaternions.size()-1); + int previousIndex = (int)Math.floor(relative); + int nextIndex = (int)Math.ceil(relative); + float percentage = relative - previousIndex; + + Quaternion previous = quaternions.get(previousIndex); + Quaternion next = quaternions.get(nextIndex); + + //interpolate between these Quaternions using the Slerp Algorithm + + double cosAngle = previous.dotProduct(next); + + double c1, c2; + // Linear interpolation for close orientations + if ((1.0 - Math.abs(cosAngle)) < 0.01) { + c1 = 1.0f - percentage; + c2 = percentage; + } else { + // Spherical interpolation + double angle = Math.acos(Math.abs(cosAngle)); + double sinAngle = Math.sin(angle); + c1 = Math.sin(angle * (1.0f - percentage)) / sinAngle; + c2 = Math.sin(angle * percentage) / sinAngle; + } + + quaternion.setX(c1 * previous.getX() + c2 * next.getX()); + quaternion.setY(c1 * previous.getY() + c2 * next.getY()); + quaternion.setZ(c1 * previous.getZ() + c2 * next.getZ()); + quaternion.setW(c1 * previous.getW() + c2 * next.getW()); + + Rotation rotation = new Rotation(quaternion.getW(), quaternion.getX(), + quaternion.getY(), quaternion.getZ(), true); + + double[] angles = rotation.getAngles(RotationOrder.XYZ); + toEdit.setYaw(Math.toDegrees(angles[0])); + toEdit.setPitch(Math.toDegrees(angles[1])); + toEdit.setRoll(Math.toDegrees(angles[2])); + } +}