diff --git a/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/floor-clip.kt b/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/floor-clip.kt index d270d784c..bc5fc9302 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/floor-clip.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/floor-clip.kt @@ -7,20 +7,28 @@ import dev.slimevr.skeleton.Skeleton import dev.slimevr.skeleton.SkeletonTargetProcessor import io.github.axisangles.ktmath.Vector3 import solarxr_protocol.datatypes.BodyPart -import kotlin.collections.set class FloorClipTargetProcessor( val skeleton: Skeleton, - val bodyParts: Array = arrayOf(BodyPart.LEFT_LOWER_LEG, BodyPart.RIGHT_LOWER_LEG), ) : SkeletonTargetProcessor { + val ankleFootParts: Array> = arrayOf( + BodyPart.LEFT_LOWER_LEG to BodyPart.LEFT_FOOT, + BodyPart.RIGHT_LOWER_LEG to BodyPart.RIGHT_FOOT, + ) + + // TODO don't just correct feet, also correct some of the hip (20% in old code). Probably to be done on IK directly. override fun process(mutableIkTargets: IKTargets, inputSkeleton: InputSkeleton, fk: ComputedSkeleton, floorLevel: Float) { if (!skeleton.effectiveFloorClip) return - for (bodyPart in bodyParts) { + for ((anklePart, footPart) in ankleFootParts) { // Get existing target or make a new one at the current bone position - val target = mutableIkTargets[bodyPart] ?: fk[bodyPart]?.tailPosition ?: continue - // Snap the target up to the floor if it's under - mutableIkTargets[bodyPart] = Vector3(target.x, target.y.coerceAtLeast(floorLevel), target.z) + val ankleTarget = mutableIkTargets[anklePart] ?: fk[anklePart]?.tailPosition ?: continue + val footTarget = mutableIkTargets[footPart] ?: fk[footPart]?.tailPosition ?: continue + + // Snap the ankle up but foot touch floor + val ankleToFoot = (ankleTarget.y - footTarget.y).coerceAtLeast(0f) + val targetY = ankleTarget.y.coerceAtLeast(floorLevel) + ankleToFoot + mutableIkTargets[anklePart] = Vector3(ankleTarget.x, targetY, ankleTarget.z) } } }