How can I make this Footplaning IK better?

The foot planting in my game looks a bit weird as the legs sort of clip through objects halfway through. This is how I want my foot planting to look:

This is my current foot planting system:

Sorry for the terrible video quality.

This is my script for the foot planting:

local hipMCF = self.RightHip.Part0.CFrame:ToWorldSpace(self.RightHipCFrame0)
		local KneeMCF = self.RightKnee.Part0.CFrame:ToWorldSpace(self.RightKneeCFrame0)
		local AnkleMCF = self.RightAnkle.Part0.CFrame:ToWorldSpace(self.RightAnkleCFrame0)
		local A = (KneeMCF.p-hipMCF.p).magnitude
		local B = (AnkleMCF.p-KneeMCF.p).magnitude + self.Character.RightFoot.Size.Y / 2	
		local hip = hipMCF.Position

		local desiredPos = (hip + down * (self.Humanoid.HipHeight)) 
		local offset =(desiredPos-hip)
		local raycastResult = workspace:Raycast(hip,offset.Unit * (offset.magnitude+1),RaycastParameters)
		local footPos = raycastResult and raycastResult.Position or (hip + offset.Unit*(offset.magnitude + self.RaycastOffset))
		
		local OriginCFrame = lowercf * self.RightHipCFrame0
		local Plane,HipAngle, KneeAngle = SolveLegIK(OriginCFrame, footPos,A,B)
		
		local NewRightHip = self.RightHipCFrame0 * OriginCFrame:ToObjectSpace(Plane) * CFrame.Angles(HipAngle, 0, 0)
		local NewRightKnee = self.RightKneeCFrame0 * CFrame.Angles(KneeAngle, 0, 0)	
		
		self.RightHip.C0 = self.RightHip.C0:Lerp(NewRightHip, DeltaTime)
		self.RightKnee.C0 = self.RightKnee.C0:Lerp(NewRightKnee, DeltaTime)
1 Like