Haverty/1771/swerve

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/input/DefaultInputs.ts

@@ -13,6 +13,10 @@ class DefaultInputs {
                 new AxisInput("arcadeDrive", "KeyW", "KeyS"),
                 new AxisInput("arcadeTurn", "KeyD", "KeyA"),
 
+                new AxisInput("swerveForward", "KeyW", "KeyS"),
+                new AxisInput("swerveStrafe", "KeyD", "KeyA"),
+                new AxisInput("swerveTurn", "ArrowRight", "ArrowLeft"),
+
                 new ButtonInput("intake", "KeyE"),
                 new ButtonInput("eject", "KeyQ"),
 

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

@@ -0,0 +1,238 @@
+import WheelDriver from "@/systems/simulation/driver/WheelDriver"
+import WheelRotationStimulus from "@/systems/simulation/stimulus/WheelStimulus"
+import Behavior from "@/systems/simulation/behavior/Behavior"
+import InputSystem from "@/systems/input/InputSystem"
+import HingeDriver from "../../driver/HingeDriver"
+import Driver, { DriverControlMode } from "../../driver/Driver"
+import HingeStimulus from "../../stimulus/HingeStimulus"
+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 { threeQuaternionToString, threeVector3ToString } from "@/util/debug/DebugPrint"
+import * as THREE from 'three'
+
+class SwerveDriveBehavior extends Behavior {
+    private _wheels: WheelDriver[]
+    private _hinges: HingeDriver[]
+    private _brainIndex: number
+    private _assemblyName: string
+
+    private _forwardSpeed = 30
+    private _strafeSpeed = 30
+    private _turnSpeed = 30
+
+    private _fieldForward: THREE.Vector3 = new THREE.Vector3(1, 0, 0)
+
+    constructor(
+        wheels: WheelDriver[],
+        hinges: HingeDriver[],
+        wheelStimuli: WheelRotationStimulus[],
+        hingeStimuli: HingeStimulus[],
+        brainIndex: number,
+        assemblyName: string
+    ) {
+        super((wheels as Driver[]).concat(hinges), (wheelStimuli as Stimulus[]).concat(hingeStimuli))
+
+        this._wheels = wheels
+        this._hinges = hinges
+        this._brainIndex = brainIndex
+        this._assemblyName = assemblyName
+
+        hinges.forEach(h => {
+            h.constraint.SetLimits(-Infinity, Infinity)
+            h.controlMode = DriverControlMode.Position
+            h.Lock()
+        })
+    }
+
+    /** @returns true if the difference between a and b is within acceptanceDelta */
+    private static withinTolerance(a: number, b: number, acceptableDelta: number) {
+        return Math.abs(a - b) < acceptableDelta
+    }
+
+    /**
+     * Creates a quaternion that represents a rotation around a specified axis by a given angle.
+     *
+     * @param angle - The angle of rotation in degrees.
+     * @param axis - The axis around which to rotate, represented as a Vector3.
+     * @returns A Quaternion representing the rotation.
+     *
+     * The function converts the angle from degrees to radians, calculates the sine and cosine
+     * of half the angle, and then constructs a quaternion from these values. The provided
+     * axis is normalized to ensure the quaternion represents a valid rotation.
+     */
+    private static angleAxis(angle: number, axis: THREE.Vector3): THREE.Quaternion {
+        const rad = (angle * Math.PI) / 180 // Convert angle to radians
+        const halfAngle = rad / 2
+        const s = Math.sin(halfAngle)
+        const normalizedAxis = axis.normalize()
+
+        return new THREE.Quaternion(Math.cos(halfAngle), normalizedAxis.x * s, normalizedAxis.y * s, normalizedAxis.z * s)
+    }
+
+    /**
+     * Applies the rotation represented by the quaternion to the given vector.
+     *
+     * @param quat - The quaternion representing the rotation.
+     * @param vec - The vector to be rotated.
+     * @returns The rotated vector as a new Vector3.
+     */
+    private static multiplyQuaternionByVector3(quat: THREE.Quaternion, vec: THREE.Vector3): THREE.Vector3 {
+        const qx = quat.x
+        const qy = quat.y
+        const qz = quat.z
+        const qw = quat.w
+        const vx = vec.x
+        const vy = vec.y
+        const vz = vec.z
+
+        // Compute quaternion-vector multiplication
+        const ix = qw * vx + qy * vz - qz * vy
+        const iy = qw * vy + qz * vx - qx * vz
+        const iz = qw * vz + qx * vy - qy * vx
+        const iw = -qx * vx - qy * vy - qz * vz
+
+        // Compute the result vector
+        return new THREE.Vector3(
+            ix * qw + iw * -qx + iy * -qz - iz * -qy,
+            iy * qw + iw * -qy + iz * -qx - ix * -qz,
+            iz * qw + iw * -qz + ix * -qy - iy * -qx
+        )
+    }
+
+    // Sets the drivetrains target linear and rotational velocity
+    private DriveSpeeds(forward: number, strafe: number, turn: number) {
+        const rootNodeId = [...World.SceneRenderer.sceneObjects.entries()]
+            .filter(x => {
+                const y = x[1] instanceof MirabufSceneObject
+                return y
+            })
+            .map(x => x[1] as MirabufSceneObject)
+            .find(o => o.assemblyName == this._assemblyName)
+            ?.GetRootNodeId()
+
+        if (rootNodeId == undefined) throw new Error("Robot root node should not be undefined")
+
+        const robotRotation = JoltQuat_ThreeQuaternion(World.PhysicsSystem.GetBody(rootNodeId).GetRotation())
+        // const robotTransform = new THREE.Matrix4()
+        // robotTransform.makeRotationFromQuaternion(robotRotation)
+
+        // const robotLocalToWorldMatrix = new THREE.Matrix4()
+
+        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.clone().sub(
+            new THREE.Vector3(0, 1, 0).multiplyScalar(new THREE.Vector3(0, 1, 0).dot(robotForward))
+        )
+
+        const headingVectorY: number = this._fieldForward.dot(headingVector)
+        const headingVectorX: number = this._fieldForward.cross(new THREE.Vector3(0, 1, 0)).dot(headingVector)
+        const chassisAngle: number = Math.atan2(headingVectorX, headingVectorY) * (180.0 / Math.PI)
+
+        forward = SwerveDriveBehavior.withinTolerance(forward, 0.0, 0.1) ? 0.0 : forward
+        strafe = SwerveDriveBehavior.withinTolerance(strafe, 0.0, 0.1) ? 0.0 : strafe
+        turn = SwerveDriveBehavior.withinTolerance(turn, 0.0, 0.1) ? 0.0 : turn
+
+        // Are the inputs basically zero
+        if (forward == 0.0 && turn == 0.0 && strafe == 0.0) {
+            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
+
+        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.normalize()
+
+        // Rotate chassis velocity by chassis angle
+        // chassisVelocity = SwerveDriveBehavior.multiplyQuaternionByVector3(
+        //     SwerveDriveBehavior.angleAxis(chassisAngle, robotUp),
+        //     chassisVelocity
+        // )
+
+        // SwerveDriveBehavior.angleAxis(chassisAngle, robotUp).setFromAxisAngle
+
+        let maxVelocity = new THREE.Vector3()
+        const com = JoltVec3_ThreeVector3(World.PhysicsSystem.GetBody(rootNodeId).GetCenterOfMassPosition())
+
+        const velocities: THREE.Vector3[] = []
+        for (let i = 0; i < this._hinges.length; i++) {
+            // TODO: We should do this only once for all azimuth drivers, but whatever for now
+            const driver = this._hinges[i]
+
+            const radius = JoltVec3_ThreeVector3(driver.worldAnchor).sub(com)
+
+            const driverAxis = JoltVec3_ThreeVector3(driver.worldAxis)
+
+            // Remove axis component of radius
+            radius.sub(driverAxis.multiplyScalar(driverAxis.dot(radius)))
+
+            velocities[i] = chassisAngularVelocity.clone().cross(radius).add(chassisVelocity)
+            if (velocities[i].length() > maxVelocity.length()) maxVelocity = velocities[i]
+        }
+
+        // Normalize all if a velocity exceeds 1
+        const maxVelocityLength = maxVelocity.length()
+        if (maxVelocityLength > 1) {
+            for (let i = 0; i < this._wheels.length; i++) {
+                velocities[i].divideScalar(maxVelocityLength)
+            }
+        }
+
+        // console.log(maxVelocity.length)
+
+        // 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])}`)
+
+            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.log(angle)
+            this._hinges[i].targetAngle = angle
+            // this._wheels[i].accelerationDirection = speed
+        }
+    }
+
+    public Update(_: number): void {
+        const forwardInput = InputSystem.getInput("swerveForward", this._brainIndex)
+        const strafeInput = InputSystem.getInput("swerveStrafe", this._brainIndex)
+        const turnInput = InputSystem.getInput("swerveTurn", this._brainIndex)
+
+        this.DriveSpeeds(
+            forwardInput * this._forwardSpeed,
+            strafeInput * this._strafeSpeed,
+            turnInput * this._turnSpeed
+        )
+    }
+}
+
+export default SwerveDriveBehavior