
com.parzivail.util.math.TwoJointIk Maven / Gradle / Ivy
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);
}
}
© 2015 - 2025 Weber Informatics LLC | Privacy Policy