nearly

src/client/java/com/iung/fpv20/flying/GlobalFlying.java

@@ -51,8 +51,8 @@ public GlobalFlying(int camRoll, float angular_speed) {
         this.drone = new DefaultDrone();
         this.droneRotation = new Quaternionf();
         this.lastDroneRotation = new Quaternionf();
-        this.cam_angel_deg = 0;
-        this.last_pos = new Vec3d(0,0,0);
+        this.cam_angel_deg = 40;
+        this.last_pos = new Vec3d(0, 0, 0);
     }
 
     public static void setFlying(boolean if_fly) {
@@ -143,7 +143,7 @@ public float handle_flying_inner(MinecraftClient client) {
 
 
         PhysicsCore.rotate_from_local_yaw_pitch_roll(q, input_y, input_p, input_r,
-                180, 180, 180,
+                300, 300, 300,
                 dt
         );
         drone.update_pose(q);
@@ -161,18 +161,61 @@ public float handle_flying_inner(MinecraftClient client) {
 //        Vec3d v0 = p.getPos();
         Vec3d pos = p.getPos();
         Vec3d v0 = pos.subtract(last_pos).multiply(1 / dt);
-        Fpv20.LOGGER.info("v0 {}",v0);
+        Fpv20.LOGGER.info("v0 {}", v0);
         last_pos = pos;
+        float aaa = 0.5f;
+        boolean to_set_x = false;
+        boolean to_set_y = false;
+        boolean to_set_z = false;
+
 
         if (Math.abs(v0.x) < 0.0001) {
             vd.x = 0;
+            to_set_y = true;
+            to_set_z = true;
         }
         if (Math.abs(v0.y) < 0.0001) {
             vd.y = 0;
+            to_set_x = true;
+            to_set_z = true;
         }
         if (Math.abs(v0.z) < 0.0001) {
             vd.z = 0;
+            to_set_y = true;
+            to_set_x = true;
         }
+
+        if (to_set_x) {
+            float d = dt * aaa;
+            if (vd.x < -d) {
+                vd.x += d;
+            } else if (vd.x > d) {
+                vd.x -= d;
+            } else {
+                vd.x = 0;
+            }
+        }
+        if (to_set_y) {
+            float d = dt * aaa;
+            if (vd.y < -d) {
+                vd.y += d;
+            } else if (vd.y > d) {
+                vd.y -= d;
+            } else {
+                vd.y = 0;
+            }
+        }
+        if (to_set_z) {
+            float d = dt * aaa;
+            if (vd.z < -d) {
+                vd.z += d;
+            } else if (vd.z > d) {
+                vd.z -= d;
+            } else {
+                vd.z = 0;
+            }
+        }
+
         drone.set_speed(new Vec3d(vd));
 
 

src/client/java/com/iung/fpv20/physics/DefaultDrone.java

@@ -1,13 +1,16 @@
 package com.iung.fpv20.physics;
 
 import com.iung.fpv20.Fpv20;
+import com.iung.fpv20.utils.FastMath;
+import net.minecraft.util.math.MathHelper;
 import net.minecraft.util.math.Vec3d;
 import org.joml.Quaternionf;
 import org.joml.Vector3f;
+import oshi.driver.mac.net.NetStat;
 
 public class DefaultDrone implements Drone {
 
-    private static final Vector3f G = new Vector3f(0,-9.8f,0);
+    private static final Vector3f G = new Vector3f(0, -9.8f, 0);
     private static final float AIR_DENSITY = 1.225F;
 
 
@@ -24,9 +27,12 @@ public class DefaultDrone implements Drone {
 
     public DefaultDrone() {
         this.pose = new Quaternionf();
-        this.mass = 20;
         this.a = new Vector3f();
         this.v = new Vector3f();
+
+        this.mass = 0.5f;
+        this.area = (float) (FastMath.PI * 0.4 * 0.4);
+        this.max_force = 5;
     }
 
     @Override
@@ -42,21 +48,38 @@ public Quaternionf get_pose() {
     @Override
     public void update_physics(float throttle, float dt) {
         Quaternionf pose = this.get_pose().conjugate();
-//        Vector3f v0 = new Vector3f(1, 0, 0).rotate(pose).mul(throttle);
-        Vector3f v1 = new Vector3f(0, 1, 0).rotate(pose).mul(throttle);
-//        Vector3f v2 = new Vector3f(0, 0, 1).rotate(pose).mul(throttle);
+        Vector3f drone_up = new Vector3f(0, 1, 0).rotate(pose);
+
+        float speed = this.v.length();
+
+
+        float dragFactor = (AIR_DENSITY * area) /
+                2F; // kg / m
+        Vector3f ambientDragForce = new Vector3f(this.v).normalize().mul(-1f *
+                speed *
+                speed *
+                dragFactor);
+        if (this.v.length() < 0.0001) {
+            ambientDragForce = new Vector3f();
+        }
+        Fpv20.LOGGER.info("#ambientDragForce {}", ambientDragForce);
+
+
+        float efficiency = MathHelper.lerp(throttle, 0.35f, 1f);
+        Vector3f thrust = new Vector3f(drone_up).mul(max_force * throttle * efficiency);
+        Fpv20.LOGGER.info("#thrust {}", thrust);
+
+        Vector3f total_force = new Vector3f().add(ambientDragForce).add(thrust);
+
+        // i don't know what's wrong, but it just falls too fast and i don't like it
+        this.a = total_force.div(mass).add(new Vector3f(G).mul(0.2f));
+        Fpv20.LOGGER.info("#a {}", this.a);
+
+        Fpv20.LOGGER.info("#f {}", dt);
 
-//        float dragFactor = (PhysicsConstants.airDensity * area * dragCoefficient) /
-//                2F; // kg / m
+        Fpv20.LOGGER.info("#dv:? {}", new Vector3f(this.a).mul(dt));
 
-//        Fpv20.LOGGER.info("x:{}",v0);
-        Fpv20.LOGGER.info("y:{}", v1);
-//        Fpv20.LOGGER.info("z:{}", v2);
-//        Fpv20.LOGGER.info("p:{}", pose);
 
-//        Vector3f v1 = new Vector3f(0, 1, 0),(this.pose).mul(throttle);
-        this.a = v1;
-//               this.pose.transformUnitPositiveY(new Vector3f()).mul(throttle);
         this.v.add(new Vector3f(this.a).mul(dt));
     }
 

src/client/java/com/iung/fpv20/utils/Calibration.java

@@ -18,7 +18,7 @@ public Calibration() {
         this.min = -1;
         this.max = 1;
         this.mid = 0;
-        this.rate = new RateMapper(0, 0);
+        this.rate = new RateMapper(0.3f, 0.5f);
         this.calibrateMethod = CalibrateMethod.MaxMidMin;
     }
 

src/main/java/com/iung/fpv20/utils/FastMath.java

@@ -1,6 +1,8 @@
 package com.iung.fpv20.utils;
 
 public class FastMath {
+    public static final float PI = (float) Math.PI;
+
     public static float abs(float fValue) {
         if (fValue < 0) {
             return -fValue;