Fixing swervedrive behavior and figuring out wheel issues

fission/src/mirabuf/MirabufSceneObject.ts

@@ -185,7 +185,8 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
                 const comMesh = World.SceneRenderer.CreateSphere(0.05)
                 World.SceneRenderer.scene.add(colliderMesh)
                 World.SceneRenderer.scene.add(comMesh)
-                ;(comMesh.material as THREE.Material).depthTest = false
+                const material = (comMesh.material as THREE.Material)
+                material.depthTest = false
                 this._debugBodies!.set(rnName, {
                     colliderMesh: colliderMesh,
                     comMesh: comMesh,

fission/src/systems/physics/PhysicsSystem.ts

@@ -28,13 +28,17 @@ import {
     PhysicsEvent,
 } from "./ContactEvents"
 import PreferencesSystem from "../preferences/PreferencesSystem"
+import { joltVec3ToString } from "@/util/debug/DebugPrint"
 
 export type JoltBodyIndexAndSequence = number
 
 export const PAUSE_REF_ASSEMBLY_SPAWNING = "assembly-spawning"
 export const PAUSE_REF_ASSEMBLY_CONFIG = "assembly-config"
 export const PAUSE_REF_ASSEMBLY_MOVE = "assembly-move"
 
+const ADAPTIVE_TIMESTEP = false
+const FIXED_TIMESTEP = 1.0 / 120.0
+
 /**
  * Layers used for determining enabled/disabled collisions.
  */
@@ -123,6 +127,7 @@ class PhysicsSystem extends WorldSystem {
         this.SetUpContactListener(this._joltPhysSystem)
 
         this._joltPhysSystem.SetGravity(new JOLT.Vec3(0, -9.8, 0))
+        // this._joltPhysSystem.SetGravity(new JOLT.Vec3(0, 0, 0))
         this._joltPhysSystem.GetPhysicsSettings().mDeterministicSimulation = false
         this._joltPhysSystem.GetPhysicsSettings().mSpeculativeContactDistance = 0.06
         this._joltPhysSystem.GetPhysicsSettings().mPenetrationSlop = 0.005
@@ -621,12 +626,17 @@ class PhysicsSystem extends WorldSystem {
         wheelSettings.mPosition = JoltRVec3_JoltVec3(anchorPoint.AddRVec3(axis.Mul(0.1)))
         wheelSettings.mMaxSteerAngle = 0.0
         wheelSettings.mMaxHandBrakeTorque = 0.0
-        wheelSettings.mRadius = radius * 1.05
+        wheelSettings.mRadius = radius * 1.00
+        // wheelSettings.mRadius = radius * 0.3
         wheelSettings.mWidth = 0.1
         wheelSettings.mSuspensionMinLength = radius * SUSPENSION_MIN_FACTOR
         wheelSettings.mSuspensionMaxLength = radius * SUSPENSION_MAX_FACTOR
+        // wheelSettings.mSuspensionMaxLength = 0.0003;
+        // wheelSettings.mSuspensionMinLength = 0.0001;
         wheelSettings.mInertia = 1
 
+        console.debug(`Wheel Position: ${joltVec3ToString(wheelSettings.mPosition)}\nRadius: ${wheelSettings.mRadius}\nMin: ${wheelSettings.mSuspensionMinLength.toFixed(5)}\nMax: ${wheelSettings.mSuspensionMaxLength.toFixed(5)}`)
+
         const vehicleSettings = new JOLT.VehicleConstraintSettings()
 
         vehicleSettings.mWheels.clear()
@@ -649,7 +659,10 @@ class PhysicsSystem extends WorldSystem {
         const fixedConstraint = JOLT.castObject(fixedSettings.Create(bodyMain, bodyWheel), JOLT.TwoBodyConstraint)
 
         // Wheel Collision Tester
+        // const tester = new JOLT.VehicleCollisionTesterCastCylinder(bodyWheel.GetObjectLayer(), 0.05)
         const tester = new JOLT.VehicleCollisionTesterCastCylinder(bodyWheel.GetObjectLayer(), 0.05)
+        // const tester = new JOLT.VehicleCollisionTesterRay(bodyWheel.GetObjectLayer(), new JOLT.Vec3(0, 1, 0))
+        // const tester = new JOLT.VehicleCollisionTesterCastSphere(bodyWheel.GetObjectLayer(), wheelSettings.mRadius)
         vehicleConstraint.SetVehicleCollisionTester(tester)
         const listener = new JOLT.VehicleConstraintStepListener(vehicleConstraint)
         this._joltPhysSystem.AddStepListener(listener)
@@ -1241,6 +1254,16 @@ class PhysicsSystem extends WorldSystem {
         })
     }
 
+    public AddConstraint(constraint: Jolt.Constraint) {
+        this._joltPhysSystem.AddConstraint(constraint)
+        this._constraints.push(constraint)
+    }
+
+    public RemoveConstraint(constraint: Jolt.Constraint) {
+        this._joltPhysSystem.RemoveConstraint(constraint)
+        this._constraints = this._constraints.filter(x => x != constraint)
+    }
+
     public GetBody(bodyId: Jolt.BodyID) {
         return this._joltPhysSystem.GetBodyLockInterface().TryGetBody(bodyId)
     }
@@ -1252,8 +1275,12 @@ class PhysicsSystem extends WorldSystem {
 
         const diffDeltaT = deltaT - lastDeltaT
 
-        lastDeltaT = lastDeltaT + Math.min(TIMESTEP_ADJUSTMENT, Math.max(-TIMESTEP_ADJUSTMENT, diffDeltaT))
-        lastDeltaT = Math.min(MAX_SIMULATION_PERIOD, Math.max(MIN_SIMULATION_PERIOD, lastDeltaT))
+        if (ADAPTIVE_TIMESTEP) {
+            lastDeltaT = lastDeltaT + Math.min(TIMESTEP_ADJUSTMENT, Math.max(-TIMESTEP_ADJUSTMENT, diffDeltaT))
+            lastDeltaT = Math.min(MAX_SIMULATION_PERIOD, Math.max(MIN_SIMULATION_PERIOD, lastDeltaT))
+        } else {
+            lastDeltaT = FIXED_TIMESTEP
+        }
 
         let substeps = Math.max(1, Math.floor((lastDeltaT / STANDARD_SIMULATION_PERIOD) * STANDARD_SUB_STEPS))
         substeps = Math.min(MAX_SUBSTEPS, Math.max(MIN_SUBSTEPS, substeps))

fission/src/systems/simulation/behavior/synthesis/SwerveDriveBehavior.ts

@@ -9,7 +9,7 @@ import Stimulus from "../../stimulus/Stimulus"
 import MirabufSceneObject from "@/mirabuf/MirabufSceneObject"
 import World from "@/systems/World"
 import { JoltMat44_ThreeMatrix4, JoltQuat_ThreeQuaternion, JoltVec3_ThreeVector3 } from "@/util/TypeConversions"
-import { threeVector3ToString } from "@/util/debug/DebugPrint"
+import { threeQuaternionToString, threeVector3ToString } from "@/util/debug/DebugPrint"
 import * as THREE from 'three'
 
 class SwerveDriveBehavior extends Behavior {
@@ -41,7 +41,8 @@ class SwerveDriveBehavior extends Behavior {
 
         hinges.forEach(h => {
             h.constraint.SetLimits(-Infinity, Infinity)
-            h.controlMode = DriverControlMode.Velocity
+            h.controlMode = DriverControlMode.Position
+            h.Lock()
         })
     }
 
@@ -113,17 +114,19 @@ class SwerveDriveBehavior extends Behavior {
 
         if (rootNodeId == undefined) throw new Error("Robot root node should not be undefined")
 
-        const robotTransform = JoltMat44_ThreeMatrix4(World.PhysicsSystem.GetBody(rootNodeId).GetWorldTransform())
+        const robotRotation = JoltQuat_ThreeQuaternion(World.PhysicsSystem.GetBody(rootNodeId).GetRotation())
+        // const robotTransform = new THREE.Matrix4()
+        // robotTransform.makeRotationFromQuaternion(robotRotation)
 
-        const robotLocalToWorldMatrix = new THREE.Matrix4()
+        // const robotLocalToWorldMatrix = new THREE.Matrix4()
 
-        const robotForward: THREE.Vector3 = new THREE.Vector3(0, 0, 1).applyMatrix4(robotTransform);
-        const robotRight: THREE.Vector3 = new THREE.Vector3(1, 0, 0).applyMatrix4(robotTransform);
-        const robotUp: THREE.Vector3 = new THREE.Vector3(0, 1, 0).applyMatrix4(robotTransform);
+        const robotForward: THREE.Vector3 = new THREE.Vector3(0, 0, 1).applyQuaternion(robotRotation);
+        const robotRight: THREE.Vector3 = new THREE.Vector3(1, 0, 0).applyQuaternion(robotRotation);
+        const robotUp: THREE.Vector3 = new THREE.Vector3(0, 1, 0).applyQuaternion(robotRotation);
 
         if (InputSystem.getInput("resetFieldForward", this._brainIndex)) this._fieldForward = robotForward
 
-        const headingVector: THREE.Vector3 = robotForward.min(
+        const headingVector: THREE.Vector3 = robotForward.clone().sub(
             new THREE.Vector3(0, 1, 0).multiplyScalar(new THREE.Vector3(0, 1, 0).dot(robotForward))
         )
 
@@ -140,30 +143,34 @@ class SwerveDriveBehavior extends Behavior {
             this._wheels.forEach(w => (w.accelerationDirection = 0.0))
             return
         } else {
+            console.debug("==================")
             console.debug(`Input: ${forward.toFixed(1)}, ${strafe.toFixed(1)}, ${turn.toFixed(1)}`)
         }
 
+        console.debug(`Robot Rotation: ${threeQuaternionToString(robotRotation, 2)}`)
         console.debug(`Robot Forward: ${threeVector3ToString(robotForward)}`)
         console.debug(`Robot Right: ${threeVector3ToString(robotRight)}`)
         console.debug(`Robot Up: ${threeVector3ToString(robotUp)}`)
 
         // Adjusts how much turning verse translation is favored
         turn *= 1.5
 
-        let chassisVelocity: THREE.Vector3 = robotForward.clone().multiplyScalar(forward).add(robotRight.clone().multiplyScalar(strafe))
+        const chassisVelocity: THREE.Vector3 = robotForward.clone().multiplyScalar(forward).add(robotRight.clone().multiplyScalar(strafe))
         const chassisAngularVelocity: THREE.Vector3 = robotUp.clone().multiplyScalar(turn)
 
         console.debug(`Lin Vel: ${threeVector3ToString(chassisVelocity)}`)
         console.debug(`Ang Vel: ${threeVector3ToString(chassisAngularVelocity)}`)
+        console.debug(`Chassis Angle: ${chassisAngle.toFixed(2)}`)
 
         // Normalize velocity so its between 1 and 0. Should only max out at like 1 sqrt(2), but still
-        if (chassisVelocity.length() > 1) chassisVelocity = chassisVelocity.normalize()
+        if (chassisVelocity.length() > 1)
+            chassisVelocity.normalize()
 
         // Rotate chassis velocity by chassis angle
-        chassisVelocity = SwerveDriveBehavior.multiplyQuaternionByVector3(
-            SwerveDriveBehavior.angleAxis(chassisAngle, robotUp),
-            chassisVelocity
-        )
+        // chassisVelocity = SwerveDriveBehavior.multiplyQuaternionByVector3(
+        //     SwerveDriveBehavior.angleAxis(chassisAngle, robotUp),
+        //     chassisVelocity
+        // )
 
         // SwerveDriveBehavior.angleAxis(chassisAngle, robotUp).setFromAxisAngle
 
@@ -175,25 +182,22 @@ class SwerveDriveBehavior extends Behavior {
             // TODO: We should do this only once for all azimuth drivers, but whatever for now
             const driver = this._hinges[i]
 
-            // TODO: driver anchor
-            const driverAnchor = new THREE.Vector3()
-
-            let radius = driverAnchor.sub(com)
+            const radius = JoltVec3_ThreeVector3(driver.worldAnchor).sub(com)
 
-            // TODO: get axis from driver
-            const driverAxis = new THREE.Vector3()
+            const driverAxis = JoltVec3_ThreeVector3(driver.worldAxis)
 
             // Remove axis component of radius
-            radius = radius.sub(driverAxis.multiplyScalar(driverAxis.dot(radius)))
+            radius.sub(driverAxis.multiplyScalar(driverAxis.dot(radius)))
 
-            velocities[i] = chassisAngularVelocity.cross(radius).add(chassisVelocity)
+            velocities[i] = chassisAngularVelocity.clone().cross(radius).add(chassisVelocity)
             if (velocities[i].length() > maxVelocity.length()) maxVelocity = velocities[i]
         }
 
         // Normalize all if a velocity exceeds 1
-        if (maxVelocity.length() > 1) {
+        const maxVelocityLength = maxVelocity.length()
+        if (maxVelocityLength > 1) {
             for (let i = 0; i < this._wheels.length; i++) {
-                velocities[i] = velocities[i].divideScalar(maxVelocity.length())
+                velocities[i].divideScalar(maxVelocityLength)
             }
         }
 
@@ -202,15 +206,15 @@ class SwerveDriveBehavior extends Behavior {
         // console.log("set speeds to " + this._hinges.length + " wheels")
 
         for (let i = 0; i < this._wheels.length; i++) {
-            // console.debug(`Velocity [${i}]: ${threeVector3ToString(velocities[i])}`)
+            console.debug(`Velocity [${i}]: ${threeVector3ToString(velocities[i])}`)
 
             const speed: number = velocities[i].length()
             const yComponent: number = robotForward.dot(velocities[i])
             const xComponent: number = robotRight.dot(velocities[i])
             const angle: number = Math.atan2(xComponent, yComponent) * (180.0 / Math.PI)
 
-            // console.debug(`Speed [${i}]: ${xComponent.toFixed(3)}, ${yComponent.toFixed(3)}`)
-            // console.debug(`Angle [${i}]: ${angle.toFixed(3)}`);
+            console.debug(`Speed [${i}]: ${xComponent.toFixed(3)}, ${yComponent.toFixed(3)}`)
+            console.debug(`Angle [${i}]: ${angle.toFixed(3)}`);
 
             //console.log(angle)
             this._hinges[i].targetAngle = angle

fission/src/systems/simulation/driver/HingeDriver.ts

@@ -1,12 +1,15 @@
 import Jolt from "@barclah/jolt-physics"
 import Driver, { DriverControlMode, DriverID } from "./Driver"
-import { GetLastDeltaT } from "@/systems/physics/PhysicsSystem"
+import PhysicsSystem, { GetLastDeltaT } from "@/systems/physics/PhysicsSystem"
 import JOLT from "@/util/loading/JoltSyncLoader"
 import { mirabuf } from "@/proto/mirabuf"
 import PreferencesSystem, { PreferenceEvent } from "@/systems/preferences/PreferencesSystem"
 import { NoraNumber, NoraTypes } from "../Nora"
+import { JoltVec3_JoltRVec3 } from "@/util/TypeConversions"
+import World from "@/systems/World"
 
-const MAX_TORQUE_WITHOUT_GRAV = 100
+// const MAX_TORQUE_WITHOUT_GRAV = 100
+const MAX_TORQUE_WITHOUT_GRAV = 0
 
 class HingeDriver extends Driver {
     private _constraint: Jolt.HingeConstraint
@@ -26,6 +29,7 @@ class HingeDriver extends Driver {
     }
     public set targetAngle(rads: number) {
         this._targetAngle = Math.max(this._constraint.GetLimitsMin(), Math.min(this._constraint.GetLimitsMax(), rads))
+        console.debug(`New target angle: ${this._targetAngle.toFixed(2)}`)
     }
 
     public get maxForce() {
@@ -61,6 +65,14 @@ class HingeDriver extends Driver {
         }
     }
 
+    public get worldAnchor(): Jolt.RVec3 {
+        return this._constraint.GetBody1().GetCenterOfMassTransform().MulVec3(this._constraint.GetLocalSpacePoint1())
+    }
+
+    public get worldAxis(): Jolt.RVec3 {
+        return this._constraint.GetBody1().GetCenterOfMassTransform().MulVec3(this._constraint.GetLocalSpaceHingeAxis1())
+    }
+
     public constructor(id: DriverID, constraint: Jolt.HingeConstraint, maxVelocity: number, info?: mirabuf.IInfo) {
         super(id, info)
 
@@ -97,21 +109,25 @@ class HingeDriver extends Driver {
             }
         }
 
-        PreferencesSystem.addEventListener(this._gravityChange)
+        // PreferencesSystem.addEventListener(this._gravityChange)
     }
 
     public Update(_: number): void {
         if (this._controlMode == DriverControlMode.Velocity) {
-            this._constraint.SetTargetAngularVelocity(this.accelerationDirection * this.maxVelocity)
+            // this._constraint.SetTargetAngularVelocity(this.accelerationDirection * this.maxVelocity)
         } else if (this._controlMode == DriverControlMode.Position) {
             let ang = this._targetAngle
 
             if (ang - this._prevAng < -this.maxVelocity) ang = this._prevAng - this.maxVelocity
             if (ang - this._prevAng > this.maxVelocity) ang = this._prevAng + this.maxVelocity
-            this._constraint.SetTargetAngle(ang)
+            // this._constraint.SetTargetAngle(ang)
         }
     }
 
+    public Lock() {
+        this._constraint.SetLimits(0, 0)
+    }
+
     public getReceiverType(): NoraTypes {
         return NoraTypes.Number
     }

fission/src/util/debug/DebugPrint.ts

@@ -38,6 +38,10 @@ export function threeVector3ToString(v: THREE.Vector3, units: number = 3) {
     return `(${v.x.toFixed(units)}, ${v.y.toFixed(units)}, ${v.z.toFixed(units)})`
 }
 
+export function threeQuaternionToString(v: THREE.Quaternion, units: number = 3) {
+    return `(${v.x.toFixed(units)}, ${v.y.toFixed(units)}, ${v.z.toFixed(units)}, ${v.w.toFixed(units)})`
+}
+
 export function joltVec3ToString(v: Jolt.Vec3 | Jolt.RVec3, units: number = 3) {
     return `(${v.GetX().toFixed(units)}, ${v.GetY().toFixed(units)}, ${v.GetZ().toFixed(units)})`
 }