skycraft: full automated landing

skycraft/index.ts

@@ -302,7 +302,45 @@ function distanceToGround(body: Body, position: number[], direction: number[]) :
     return {distance, normal};
 }
 
-function autoLand(context) {
+function finishLanding(context, body) {
+    const p = context.player;
+    context.landed = true;
+    context.landedBody = body;
+
+    p.tf = [
+        se3.inverse(body.orientation),
+        se3.inverse(se3.translation(...body.position)),
+        p.tf,
+    ].reduce(se3.product);
+    p.velocity = [0, 0, 0];
+    p.position = se3.apply(p.tf, [0, 0, 0, 1]);
+}
+
+function alignUp(playerTf, surfaceNormal) {
+    const zaxis = se3.apply(playerTf, [0, 0, 1, 0]);
+    const ny = surfaceNormal;
+    const nx = linalg.normalize(linalg.cross(ny, zaxis));
+    const nz = linalg.normalize(linalg.cross(nx, ny));
+
+    return se3.fromBases(nx, ny, nz);
+}
+
+function calculateLandingDv(verticalSpeed, altitude, dt) {
+    // pretty crude logic, could be improved
+    const maxThrust = 2;
+    const final = verticalSpeed**2 / (2*(altitude - 1.0));
+
+    if (verticalSpeed > 0 || final < maxThrust * 0.8) {
+        // can still accelerate forward
+        return -maxThrust * dt;
+    } else if (final < maxThrust) {
+        // coast
+        return 0;
+    }
+    return final;
+}
+
+function autoLand(context, dt) {
     const p = context.player;
     // 0. check prereqs:
     //   - none
@@ -317,44 +355,32 @@ function autoLand(context) {
     const toBodyCenter = linalg.diff(body.position, p.position);
     const {distance: altitude, normal: surfaceNormal} = distanceToGround(body, p.position, linalg.normalize(toBodyCenter));
 
-    // figure out tangential speed
+    const surfaceVelocity = linalg.add(body.velocity, linalg.cross(body.spin, linalg.scale(toBodyCenter, -1)));
-    const spinSpeed = linalg.add(body.velocity, linalg.cross(body.spin, linalg.scale(toBodyCenter, -1)));
+    const relativeVelocity = linalg.diff(p.velocity, surfaceVelocity);
-    const dv = linalg.diff(spinSpeed, p.velocity);
+    const upward = linalg.dot(relativeVelocity, surfaceNormal);
-    // remove vertical component
+    const vertical = linalg.scale(surfaceNormal, upward);
-    const vertical = linalg.scale(surfaceNormal, -linalg.dot(dv, surfaceNormal));
+    const horizontal = linalg.diff(relativeVelocity, vertical);
-    const smallDv = linalg.scale(linalg.add(dv, vertical), 0.01);
+    const smallDv = linalg.scale(horizontal, -0.01);
-//    const smallDv = linalg.scale(dv, 0.05);
     p.velocity = linalg.add(p.velocity, smallDv);
 
     // up is up
-    const shipZ = se3.apply(p.tf, [0, 0, 1, 0]);
+    const currentOrientation = slerp(p.tf, alignUp(p.tf, surfaceNormal), kShipRotateSpeed);
-    const ny = surfaceNormal;
-    const nx = linalg.normalize(linalg.cross(ny, shipZ));
-    const nz = linalg.normalize(linalg.cross(nx, ny));
-
-    const targetOrientation = se3.fromBases(nx, ny, nz);
-    const currentOrientation = slerp(p.tf, targetOrientation, kShipRotateSpeed);
     p.tf = se3.setOrientation(p.tf, currentOrientation);
 
     if (altitude < 1.5) {
-        const upward = linalg.dot(linalg.diff(p.velocity, body.velocity), surfaceNormal);
         if (upward < -2) {
+            // going too fast, bounce
             p.velocity = linalg.add(p.velocity, linalg.scale(surfaceNormal, -2*upward));
             context.landing = false;
         } else if (upward < 0) {
-            context.landed = true;
+            finishLanding(context, body);
-            context.landedBody = body;
+            return;
-
-            p.tf = [
-                se3.inverse(body.orientation),
-                se3.inverse(se3.translation(...body.position)),
-                p.tf,
-            ].reduce(se3.product);
-            p.velocity = [0, 0, 0];
-            p.position = se3.apply(p.tf, [0, 0, 0, 1]);
         }
     }
-//    p.velocity = linalg.add(p.velocity, linalg.scale(surfaceNormal, dvup));
+
+    // accelerate towards the ground and then slow down to land
+    const dvup = calculateLandingDv(upward, altitude, dt);
+    p.velocity = linalg.add(p.velocity, linalg.scale(surfaceNormal, dvup));
 }
 
 function effectiveGravity(position: number[], rootBody: Body) : number[] {
@@ -399,9 +425,9 @@ function updatePhysics(time: number, context) {
     }
 
     if (context.landing) {
-        autoLand(context);
+        autoLand(context, dt);
     } else {
-        // allow acdjusting camera
+        // allow adjusting camera
         const dr = slerp(se3.identity(), context.camera.tf, kShipRotateSpeed);
         player.tf = se3.product(player.tf, dr);
         context.camera.tf = se3.product(context.camera.tf, se3.inverse(dr));