I'm trying to make a 3d, 2d platformer physics based puzzel semi-horrer game and found that rigid bodies are pretty glitchy and easily can get pushed in other objects with higher mass or objects that are static and also 3d physics collision objects are hard to work with when they need to constantly stay on the same 2d plane. So I decided to make a custom pluggin node that inherits the ShapeCast3D node. The custom physics node I made has been given mass and velocity as important members. Each frame a force of gravity is given to the node to cause it to fall. Before the object moves each frame it casts out a shape cast to predict collisions before the object intersects with other objects. When the shape cast detects an object I return the object it detects, the collision point (which I alter to move it in the trajectory of the physics object) and the collision normal. Using the data provided, I move the physics object over where the object would be during collision (not accurate placement going to get to that later) and then I have the physics object transfer momentum into the object if collides into (only works with other custom physics objects). My problem is that my physics objects tend to glitch into eachother if they have commonly if they both have momentum. No matter how many tweaks I do this issue tends to remain unresolved. My second problem is that when I move the object to where they would be during collision the placement is not at all accurate because I'm at the moment required to have a volume variable which is not the best method for objects with different shapes and sizes and the math done to move the object could still result in the object moving into the object it collided with or and extra unwanted distance. I see that when the shape cast is used it does show where the collision shape of the shape cast would be during the collision (I'm calling the normal position) but I have failed to find out how get the variable or function to get the normal position.

@tool
extends ShapeCast3D

#Properties that can be editted in editor
@export_category("Physical Properties")
@export var Mass : float = 1
@export var ForceOfGravity := Vector2(0,-9.8)
@export_range(0.0,1.0) var ForceConversion : float = 0
@export var IsStatic : bool = false #if true, te object would be treated as if it had infinite mass, any collision applied to a static object would be completely returned and the force this object would apply would be the velocity times the mass of the other object
@export var Velocity := Vector2(0,0)

#@export_category("Editor Settings")
#@export var SimulateInEditor = false #refresh editor if selected

#Adds collider to allow other objects to collide with this one
var Collider = Area3D.new()
var ColliderShape = CollisionShape3D.new()

func _enter_tree():
	
	target_position = Vector3(0,0,0)
	
	#Runs code when in editor
	if !Engine.is_editor_hint():
		
		set_process(true)
		
		#Add collider as child of TSPhysObj
		Collider.add_child(ColliderShape) 
		ColliderShape.shape = shape
		add_child(Collider)
		
		#Allows collision with Areas
		collide_with_areas = true
		
		#Prevents collision with own collider
		add_exception(Collider)
	

func _ready():
	pass

var Ready = false

const DebugFrameRate = 0
var Counter = 0

func _process(delta):
	
	if !Ready:
		Ready = Input.is_anything_pressed()
		return
	
	if DebugFrameRate > 0:
		Counter += delta
		
		if Counter >= 1.0/DebugFrameRate:
			Counter -= 1.0/DebugFrameRate
		else:
			return
	
	#Prevents code from running in editor
	if Engine.is_editor_hint():
		set_process(false)
		return
	
	ApplyForce(Mass,ForceOfGravity*delta)
	
#	if IsStatic:
#		Velocity = Vector2(0,0)
	
	if Velocity != Vector2(0,0):
		MoveAndCollide(delta)
		#target_position = Vec2ToYZ(RotatePoint(Velocity*delta*VelocityDelta,-rotation.x))

func ApplyForce(AppliedMass:=0,AppliedAcceleration:=Vector2(0,0)):
	Velocity+=(AppliedAcceleration*AppliedMass)/Mass

var VirtualPosition = Vector2(0,0) 
var VelocityDelta = 1
var Collisions = 0

func MoveAndCollide(delta,Initial = true):
	
	if Initial:
		VelocityDelta = 1 #used to help more accurately move the object during a chain of collisions
		VirtualPosition = YZToVec2(global_position)
	
	var WasCollision = Collide(delta)
	
#	if WasCollision && VelocityDelta > 0 && Collisions < 10: #More than ten is proof of compression
#
#		Collisions += 1
#
#		MoveAndCollide(delta,false)
#
#		if Collisions > 1:
#			print(Collisions)
#
#		return
	
	global_position = Vec2ToYZ(VirtualPosition)
	
	if VelocityDelta > 0:
		global_position += Vec2ToYZ(Velocity * delta * VelocityDelta)
	
	Collisions = 0

var Volume = Vector2(1,1) #area

func Collide(delta):
	
	var OriginalPos = global_position
	global_position = Vec2ToYZ(VirtualPosition)
	
	target_position = to_local(global_position+Vec2ToYZ(Velocity*delta*VelocityDelta)) #re orientates the velocity so the target position points in the correct direction
	force_shapecast_update()
	
	global_position = OriginalPos
	
	if is_colliding():
		
		var CollisionPoint = MovePointOnCourse(YZToVec2(get_collision_point(0)),VirtualPosition,Velocity*delta*VelocityDelta)#It moves the collision point on the objects course since shape casters don't give a easily usable collision point vector
		var CollisionNormal = YZToVec2(get_collision_normal(0))
		
#		#plays out movement prior to collision
#		var CorneredCollisionNormal = (CollisionNormal/abs(CollisionNormal.y)) if abs(CollisionNormal.y) > abs(CollisionNormal.x) else (CollisionNormal/abs(CollisionNormal.x))
#		#print(CollisionNormal,CorneredCollisionNormal)
#		var PositionDuringCollision = CollisionPoint + CorneredCollisionNormal * (Volume+Vector2(.8,.8))/2
#		VelocityDelta -= GetTimeToDestination(PositionDuringCollision,YZToVec2(position),Velocity*delta*VelocityDelta)
#		VirtualPosition = PositionDuringCollision
		
		#Adds Collision force
		TransferMomentum(CollisionNormal,get_collider(0))
		
		return true #Was collision
	return false #was no collision

func GetTimeToDestination(Point,Origin,Direction):
	#Shows how many frames it takes for an object to get to a destination 
	var DeltaTime = Vector2((Point.x-Origin.x)/Direction.x,(Point.y-Origin.y)/Direction.y)
	
	#Handles cases where DeltaTime < 0 or is infinite and chooses the best of the two if they satisfy all conditions
	if DeltaTime.x > 0 && !is_inf(DeltaTime.x) && (DeltaTime.x <= DeltaTime.y || DeltaTime.y >0):
		return DeltaTime.x
	elif DeltaTime.y > 0 && !is_inf(DeltaTime.y) && (DeltaTime.y <= DeltaTime.x || DeltaTime.x >0):
		return DeltaTime.y
	else:
		return 0
	

func MovePointOnCourse(Point,Origin,Direction):
	#Shows how many frames it takes for an object to get to a destination 
	var DeltaTime = GetTimeToDestination(Point,Origin,Direction)
	
	return Origin+Direction*DeltaTime
	


func TransferMomentum(Normal,InvolvedCollider):
	
	var InvolvedColliderParent = InvolvedCollider.get_parent()
	
	#Helps determine how collision would act in a result of the objects being static
	var Object1Static = IsStatic
	var Object2Static = false
	
	if "IsStatic" in InvolvedColliderParent:
		Object2Static = InvolvedColliderParent.IsStatic
	else:
		Object2Static = true
	
	
	var Mass1 = Mass
	var Mass2 = InvolvedColliderParent.Mass
	
	var Velocity1 = Velocity 
	var Velocity2 = InvolvedColliderParent.Velocity
	
	#Redirects collision based on normal
	var Radians = atan2(Normal.y,Normal.x)
	
	var RotatedVelocity1 = -RotatePoint(Velocity1,Radians)
	var RotatedVelocity2 = -RotatePoint(Velocity2,Radians)
	
	#Transfered Momentum Velocity
	var TMVelocity1 = Vector2(
		((Mass1 - Mass2) * RotatedVelocity1.x + 2 * Mass2 * RotatedVelocity2.x) / (Mass1 + Mass2),
		((Mass1 - Mass2) * RotatedVelocity1.y + 2 * Mass2 * RotatedVelocity2.y) / (Mass1 + Mass2)
	)
	
	var TMVelocity2 = Vector2(
		((Mass2 - Mass1) * RotatedVelocity2.x + 2 * Mass1 * RotatedVelocity1.x) / (Mass1 + Mass2),
		((Mass2 - Mass1) * RotatedVelocity2.y + 2 * Mass1 * RotatedVelocity1.y) / (Mass1 + Mass2)
	)
	
	InvolvedColliderParent.ApplyForce(Mass2,TMVelocity2-Velocity2)#
	
	ApplyForce(Mass1,TMVelocity1-Velocity1)#
	
##	print(Normal)
#
#	#Changes the shape of the normal range from a circle to a square
#	var CorneredNormal = (Normal * 1/Normal.y) if abs(Normal.y) > abs(Normal.x) else (Normal * 1/Normal.x) # (.707,.707) becomes (1,1) or top right most of circle is converted to top right corner of square of same height
#	#(Normal/abs(Normal.x)) if abs(Normal.x) > abs(Normal.y) else (Normal/abs(Normal.y))
#
##	print(CorneredNormal)
#
#	#Sets velocity that is involved in the linear collision
#	var DirectedVelocity1 = Velocity * CorneredNormal
#	var DirectedVelocity2 = InvolvedColliderParent.Velocity * CorneredNormal
#
##	print(DirectedVelocity1)
##	print(DirectedVelocity2)
#
#	#Sets velocity as if it is in a one dimensional line
#	var LinearVelocity1 = sqrt(pow(DirectedVelocity1.x,2)+pow(DirectedVelocity1.y,2))*-1 #inverted because direction would usually be pointing away from current velocity
#	var LinearVelocity2 = sqrt(pow(DirectedVelocity2.x,2)+pow(DirectedVelocity2.y,2))
#
##	print(LinearVelocity1)
##	print(LinearVelocity2)
#
#	var Mass1 = Mass
#	var Mass2 = InvolvedColliderParent.Mass
#
#	var NewLinearVelocity1 = ((Mass1 - Mass2) * LinearVelocity1 + 2 * Mass2 * LinearVelocity2) / (Mass1 + Mass2)# * Mass1
#	var NewLinearVelocity2 = ((Mass2 - Mass1) * LinearVelocity2 + 2 * Mass1 * LinearVelocity1) / (Mass2 + Mass1)# * Mass2
#
##	print(NewLinearVelocity1)
##	print(NewLinearVelocity2)
#
#	var NewDirectedVelocity1 = NewLinearVelocity1*Normal
#	var NewDirectedVelocity2 = NewLinearVelocity2*Normal
#
#	InvolvedColliderParent.ApplyForce(Mass2,NewDirectedVelocity2-DirectedVelocity2)
#
#	ApplyForce(Mass1,NewDirectedVelocity1-DirectedVelocity1)
#
#
#

#Allows Vec2 to be converted into the YZ slice of Vec3
func Vec2ToYZ(Vec2:=Vector2(0,0)):
	return Vector3(0,Vec2.y,Vec2.x)

#Allows YZ slice of Vec3 to convert into Vec2
func YZToVec2(Vec3:=Vector3(0,0,0)):
	return Vector2(Vec3.z,Vec3.y)

func RotatePoint(Vec2:=Vector2(0,0),Radians:float=0):
	var DotProduct = -2*(Vec2.x*cos(Radians)+Vec2.y*sin(Radians))
	return Vector2(Vec2.x+DotProduct*cos(Radians),Vec2.y+DotProduct*sin(Radians))

If you have a question about my code let me know. Also if their is a way around my problem as if a function of a static-rigid hybrid node exists are a way to prevent heavier objects from pushing lighter objects into other heavy objects let me know.