com.parzivail.util.math.TwoJointIk Maven / Gradle / Ivy

Go to download
package com.parzivail.util.math;

import D;
import I;
import net.minecraft.class_1297;
import net.minecraft.class_1937;
import net.minecraft.class_239;
import net.minecraft.class_243;
import net.minecraft.class_3532;
import net.minecraft.class_3959;
import net.minecraft.class_3965;
import org.jetbrains.annotations.NotNull;

public class TwoJointIk
{
	public record Result(class_243 kneePos, class_243 footPos, double hipYaw, double hipPitch, double kneePitch)
	{
	}

	public static Result forwardEvaluate(class_1297 entity, class_243 hip, boolean backwardsKnee, double hipYaw, double upperLegLength, double lowerLegLength)
	{
		var entityPos = entity.method_19538();

		var lTotal = upperLegLength + lowerLegLength;

		var world = entity.method_37908();
		var result = world.method_17742(new class_3959(hip.method_1019(entityPos), hip.method_1019(entityPos).method_1023(0, lTotal, 0), class_3959.class_3960.field_17558, class_3959.class_242.field_1348, entity));
		if (result.method_17783() == class_239.class_240.field_1333)
			return new Result(class_243.field_1353, class_243.field_1353, hipYaw, 0, 0);

		var footPos = result.method_17784().method_1020(entityPos);
		var dGround = hip.field_1351 - footPos.field_1351 - lowerLegLength;
		var hipPitch = 90 - (Math.asin(dGround / upperLegLength)) / Math.PI * 180;

		var kneePos = footPos.method_1031(0, lowerLegLength, 0);

		return new Result(kneePos, footPos, hipYaw, hipPitch, hipPitch);
	}

	public static Result backEvaluate(class_1297 entity, class_243 hip, class_243 footRequest, boolean backwardsKnee, double upperLegLength, double lowerLegLength)
	{
		var entityPos = entity.method_19538();

		var lTotal = upperLegLength + lowerLegLength;

		var hipYaw = hip.field_1352 == footRequest.field_1352 && hip.field_1350 == footRequest.field_1350 ? entity.method_36454() : class_3532.method_15349(footRequest.field_1350 - hip.field_1350, footRequest.field_1352 - hip.field_1352) / Math.PI * 180 - 90;

		if (hip.method_1022(footRequest) >= lTotal)
			return getFullyExtendedResult(hip, footRequest, hipYaw);

		var world = entity.method_37908();
		var result = world.method_17742(new class_3959(footRequest.method_1019(entityPos), footRequest.method_1019(entityPos).method_1023(0, lTotal, 0), class_3959.class_3960.field_17558, class_3959.class_242.field_1348, entity));
		if (result.method_17783() == class_239.class_240.field_1333)
			return getFullyExtendedResult(hip, footRequest, hipYaw);

		footRequest = result.method_17784().method_1020(entityPos);

		footRequest = footRequest.method_1020(hip);

		var endX = footRequest.method_37267();
		var endY = footRequest.field_1351;

		var lSquareDiff = upperLegLength * upperLegLength - lowerLegLength * lowerLegLength;
		var lSquareSum = upperLegLength * upperLegLength + lowerLegLength * lowerLegLength;
		var dSquare = footRequest.method_1027();
		var coef = lSquareDiff / (2 * dSquare);
		var coef2 = Math.sqrt(2 * (lSquareSum / dSquare) - lSquareDiff * lSquareDiff / (dSquare * dSquare) - 1) / 2f;

		var opposite = backwardsKnee ? -1 : 1;
		var intersectionX = endX / 2 + coef * endX - opposite * coef2 * endY;
		var intersectionY = endY / 2 + coef * endY + opposite * coef2 * endX;

		var hipPitch = class_3532.method_15349(intersectionY, intersectionX) / Math.PI * 180;
		var kneePitch = hipPitch - class_3532.method_15349(endY - intersectionY, endX - intersectionX) / Math.PI * 180;

		var hipYawRad = hipYaw / 180 * Math.PI;

		var kneePos = new class_243(intersectionX * Math.cos(hipYawRad), intersectionY, intersectionX * Math.sin(hipYawRad));
		return new Result(kneePos, footRequest, hipYaw, 90 + hipPitch, kneePitch);
	}

	@NotNull
	private static TwoJointIk.Result getFullyExtendedResult(class_243 hip, class_243 footRequest, double hipYaw)
	{
		var deltaPos = footRequest.method_1020(hip);
		var fullyExtendedHipPitch = class_3532.method_15349(deltaPos.field_1351, deltaPos.method_37267());
		return new Result(class_243.field_1353, class_243.field_1353, hipYaw, fullyExtendedHipPitch, 0);
	}
}