extends SceneTree
## Does the shipping SphereSweep report the RIGHT ground point and the RIGHT
## surface normal at the altitudes a 917 km world puts you at?
##
## WHY THIS EXISTS. Foot IK was reported garbled on the big maps. sweep() makes
## three physics calls -- cast_motion, get_rest_info at the contact pose, and
## get_rest_info again after a slide refinement -- and silently falls back to a
## GUESS when the middle one comes back empty. This gate measures the guess.
##
## THE GROUND IS SLOPED, and that is the whole design of this gate. The fallback
## invents `normal = -motion.normalized()`, which on FLAT ground is straight up
## and therefore ACCIDENTALLY CORRECT -- a flat-ground gate would certify a
## broken normal. The slope is what makes the normal falsifiable. (A flat row is
## kept alongside it, because the HEIGHT error is clearest there.)
##
## Both grounds are the shipping arrangement: chunk-local faces with the world
## offset in the body transform, as dc_collision_trimesh.h:176-191 builds them.
##
## Run:
## "$HYPER_TPS_GODOT" --headless --script res://scenes/test/large_coord_ik_probe.gd
const R := 0.05 # foot_ik.gd:20 SPHERE_RADIUS
const UP := 0.2 # foot_ik.gd:21 SWEEP_UP
const DOWN := 0.6 # foot_ik.gd:22 SWEEP_DOWN
## Rise per metre of +X. 30 degrees: steep enough that a wrong normal is obvious,
## shallow enough that the downward sweep still lands on the face.
const SLOPE := 0.5773502692 # tan(30 deg)
## Offsets, applied to ONE axis at a time. Horizontal and vertical magnitude are
## different questions and the first version of this gate conflated them.
var _offsets: Array = [0.0, 1000.0, 4000.0, 9700.0, 32000.0, 100000.0]
var _warm: int = 0
var _fail: int = 0
func _initialize() -> void:
for d in _offsets:
_build(d, true, false) # X offset, flat
_build(d, false, false) # Y offset, flat
_build(d, true, true) # X offset, sloped
_build(d, false, true) # Y offset, sloped
## The bodies must be in the BROADPHASE before any query sees them, and that
## needs a physics step. An earlier version queried inside _initialize and read a
## clean miss at EVERY offset INCLUDING the origin -- the instrument, not the
## world, and the origin row is what said so.
func _physics_process(_d: float) -> bool:
_warm += 1
if _warm < 3:
return false
print("")
print("FLAT ground -- point must lie on the plane, normal (0, 1, 0)")
_table(false)
print("")
print("SLOPED ground (30 deg) -- point must lie on the slope, normal (-0.5, 0.866, 0)")
_table(true)
print("")
if _fail == 0:
print("PASS every offset returns the surface point and the real normal")
else:
print("FAIL %d rows wrong" % _fail)
return true
func _table(sloped: bool) -> void:
print("offset(m) large X (altitude small) large Y (altitude)")
print("------------------------------------------------------------------------------------------------")
for d in _offsets:
print("%9.0f %-42s %s" % [d, _sweep(d, true, sloped), _sweep(d, false, sloped)])
## A 40 x 40 m patch, chunk-local, optionally tilted about Z so height varies
## with local X.
func _build(d: float, x_axis: bool, sloped: bool) -> void:
var g := StaticBody3D.new()
var cs := CollisionShape3D.new()
var f := PackedVector3Array()
var s := 0.0
if sloped:
s = SLOPE
var i := 0
while i < 20:
var j := 0
while j < 20:
var x0 := float(i) * 2.0 - 20.0
var z0 := float(j) * 2.0 - 20.0
var x1 := x0 + 2.0
var z1 := z0 + 2.0
var y0 := x0 * s
var y1 := x1 * s
# CW, so the face normal points UP. Godot's Plane(p1,p2,p3) normal is
# (p1-p3).cross(p1-p2); the obvious order gives (0,-1,0) and the patch
# then only collides from BELOW. [[feedback_godot-cw-winding]]
f.push_back(Vector3(x0, y0, z0)); f.push_back(Vector3(x1, y1, z1)); f.push_back(Vector3(x0, y0, z1))
f.push_back(Vector3(x0, y0, z0)); f.push_back(Vector3(x1, y1, z0)); f.push_back(Vector3(x1, y1, z1))
j += 1
i += 1
var tri := ConcavePolygonShape3D.new()
# ONE-SIDED, as dc_collision_trimesh.h builds it -- it never sets
# backface_collision. Turning it on to dodge a winding mistake also lets
# get_rest_info answer with the UNDERSIDE face: this gate measured a normal
# 180 deg out and that was the rig, not the engine.
tri.set_faces(f)
cs.shape = tri
g.add_child(cs)
# Sloped and flat patches are separated in Z so they cannot answer for each
# other, and the sweep column below matches the separation.
var zoff := 0.0
if sloped:
zoff = 200.0
if x_axis:
g.position = Vector3(d, 0.0, zoff)
else:
g.position = Vector3(0.0, d, zoff)
root.add_child(g)
## The SHIPPING call, not a hand-rolled approximation of it.
func _sweep(d: float, x_axis: bool, sloped: bool) -> String:
var space: PhysicsDirectSpaceState3D = root.get_world_3d().direct_space_state
var zoff := 0.0
if sloped:
zoff = 200.0
var origin := Vector3(d, 0.0, zoff)
if not x_axis:
origin = Vector3(0.0, d, zoff)
# Probe one metre along local +X so a sloped patch has a non-zero height there.
var lx := 1.0
var s := 0.0
if sloped:
s = SLOPE
var true_y: float = origin.y + lx * s
var true_n := Vector3(0.0, 1.0, 0.0)
if sloped:
true_n = Vector3(-s, 1.0, 0.0).normalized()
var foot := Vector3(origin.x + lx, true_y + 0.05, origin.z)
# The invariant, and it holds on both grounds: the returned point LIES ON
# THE SURFACE. Where along the surface it lands is geometry, not error.
var ex: Array[RID] = []
var res: Dictionary = SphereSweep.sweep(space, foot + Vector3(0, UP, 0),
foot - Vector3(0, DOWN, 0), R, ex)
if res.is_empty():
_fail += 1
return "NO HIT AT ALL"
var hp: Vector3 = res["point"]
var n: Vector3 = res["normal"]
# Residual against the surface the patch actually defines:
# y = origin.y + s * (x - origin.x)
var on_surface: float = hp.y - (origin.y + s * (hp.x - origin.x))
var nerr: float = rad_to_deg(n.angle_to(true_n))
var bad := false
if absf(on_surface) > 0.005:
bad = true
if nerr > 2.0:
bad = true
if bad:
_fail += 1
var mark := " "
if bad:
mark = "<<"
return "off-surface %+.5f m n_err %5.1f deg %s" % [on_surface, nerr, mark]