diff options
| author | Fuwn <[email protected]> | 2026-02-12 22:33:32 -0800 |
|---|---|---|
| committer | Fuwn <[email protected]> | 2026-02-12 22:33:32 -0800 |
| commit | c7a9d4a6bd53ed7d61731770f2f10e8b9fd435f9 (patch) | |
| tree | df9f48bf128a6c0186a8e91857d6ff30fe0e9f18 /NET/worlds/scape/VehicleDriver.java | |
| download | worldsplayer-c7a9d4a6bd53ed7d61731770f2f10e8b9fd435f9.tar.xz worldsplayer-c7a9d4a6bd53ed7d61731770f2f10e8b9fd435f9.zip | |
Initial commit
Diffstat (limited to 'NET/worlds/scape/VehicleDriver.java')
| -rw-r--r-- | NET/worlds/scape/VehicleDriver.java | 653 |
1 files changed, 653 insertions, 0 deletions
diff --git a/NET/worlds/scape/VehicleDriver.java b/NET/worlds/scape/VehicleDriver.java new file mode 100644 index 0000000..10daf6c --- /dev/null +++ b/NET/worlds/scape/VehicleDriver.java @@ -0,0 +1,653 @@ +package NET.worlds.scape; + +public class VehicleDriver extends SwitchableBehavior implements MouseDeltaHandler, KeyUpHandler, KeyDownHandler, MouseDownHandler, MouseUpHandler, FrameHandler { + protected VehicleShape vehicle; + static final boolean debug = true; + static final float gravity = 32.1F; + static final float densityOfAir = 0.0801F; + static final float dragCoefficient = 0.3F; + static final float feetToWorld = 30.48F; + static final float worldToFeet = 0.0328084F; + static final float epsilon = 0.001F; + static final float maxCamber = 0.4F; + static final float rotationalDampener = 0.5F; + static final int asphalt = 0; + static final int grass = 1; + static final int numTerrainTypes = 2; + float acceleratorDepression = 0.0F; + float brakesDepression = 0.0F; + int currentGear = 1; + float steeringWheelPosition = 0.0F; + boolean gasKeyDown = false; + boolean brakeKeyDown = false; + boolean leftKeyDown = false; + boolean rightKeyDown = false; + Point3 velocityVector = new Point3(0.0F, 0.0F, 0.0F); + Point3 velocityVectorCarFrame = new Point3(0.0F, 0.0F, 0.0F); + float velocity = 0.0F; + Point3 angularVelocity = new Point3(0.0F, 0.0F, 0.0F); + Point3 angularVelocityCarFrame = new Point3(0.0F, 0.0F, 0.0F); + Point3 worldCenterOfMass = new Point3(0.0F, 0.0F, 0.0F); + float engineRPM = 0.0F; + Point3 integratedForce = new Point3(0.0F, 0.0F, 0.0F); + Point3 integratedTorque = new Point3(0.0F, 0.0F, 0.0F); + boolean disabled = false; + float[] minRPMs; + VehicleDriver.Tire[] tires; + protected Room room; + protected Pilot pilot; + static float lastTime = 0.0F; + static final int lateralForceIncrements = 1000; + static float[][] muTable; + static boolean lateralForceTableInited = false; + static final int torqueIncrements = 500; + static final float maxRPM = 7000.0F; + static final float minRPM = 1500.0F; + static final float rpmInc = 500.0F; + static final int numRPMs = 12; + static float[] torqueTable; + static boolean torqueTableInited = false; + static float[] rawTorqueData = new float[]{80.0F, 82.0F, 91.0F, 92.0F, 89.0F, 90.0F, 98.0F, 93.0F, 94.0F, 78.0F, 83.0F, 75.0F}; + static final float torqueMax = 98.0F; + + public VehicleDriver(VehicleShape p_Vehicle) { + this.vehicle = p_Vehicle; + this.initLateralForceTable(); + this.initTorqueTable(); + this.initRPMTable(); + this.tires = new VehicleDriver.Tire[4]; + + for (int tire = 0; tire < 4; tire++) { + this.tires[tire] = new VehicleDriver.Tire(); + this.tires[tire].relativePos.copy(this.vehicle.tirePositions[tire]); + } + } + + @Override + public boolean handle(MouseDeltaEvent e) { + return UniverseHandler.handle(e); + } + + @Override + public boolean handle(KeyDownEvent e) { + if (UniverseHandler.handle(e)) { + return true; + } else { + switch (e.key) { + case '1': + if (this.vehicle.stickShift) { + this.currentGear = 1; + } + break; + case '2': + if (this.vehicle.stickShift) { + this.currentGear = 2; + } + break; + case '3': + if (this.vehicle.stickShift) { + this.currentGear = 3; + } + break; + case '4': + if (this.vehicle.stickShift) { + this.currentGear = 4; + } + break; + case '5': + if (this.vehicle.stickShift) { + this.currentGear = 5; + } + break; + case 'R': + case 'r': + this.currentGear = 0; + break; + case '\ue325': + this.leftKeyDown = true; + break; + case '\ue326': + this.gasKeyDown = true; + break; + case '\ue327': + this.rightKeyDown = true; + break; + case '\ue328': + this.brakeKeyDown = true; + } + + return true; + } + } + + @Override + public boolean handle(KeyUpEvent e) { + if (UniverseHandler.handle(e)) { + return true; + } else { + switch (e.key) { + case '\ue325': + this.leftKeyDown = false; + break; + case '\ue326': + this.gasKeyDown = false; + break; + case '\ue327': + this.rightKeyDown = false; + break; + case '\ue328': + this.brakeKeyDown = false; + } + + return true; + } + } + + @Override + public boolean handle(MouseDownEvent e) { + return false; + } + + @Override + public boolean handle(MouseUpEvent e) { + return false; + } + + @Override + public boolean handle(FrameEvent e) { + if (!(e.receiver instanceof Pilot)) { + return true; + } else { + this.pilot = (Pilot)e.receiver; + if (!this.pilot.isActive()) { + return true; + } else { + Point3Temp com = Point3Temp.make(this.vehicle.centerOfGravity); + com.times(30.48F); + com.times(this.pilot.getObjectToWorldMatrix()); + com.times(0.0328084F); + this.worldCenterOfMass.set(com.x, com.y, com.z); + this.room = this.pilot.getRoom(); + int now = e.time; + float dt = (now - lastTime) / 1000.0F; + lastTime = now; + if (dt <= 0.0F) { + return true; + } else { + if (dt > 0.33F) { + dt = 0.33F; + } + + if (this.gasKeyDown) { + this.acceleratorDepression = (float)(this.acceleratorDepression + 1.2 * dt); + } else { + this.acceleratorDepression = 0.0F; + } + + if (this.acceleratorDepression > 1.0) { + this.acceleratorDepression = 1.0F; + } + + if (this.brakeKeyDown) { + this.brakesDepression = (float)(this.brakesDepression + 1.2 * dt); + } else { + this.brakesDepression = 0.0F; + } + + if (this.brakesDepression > 1.0) { + this.brakesDepression = 1.0F; + } + + float wheelDelta = 0.282F * dt; + if (Math.abs(this.velocityVectorCarFrame.y) < 10.0F) { + wheelDelta = (float)(wheelDelta * 0.5); + } + + if (this.leftKeyDown) { + this.steeringWheelPosition -= wheelDelta; + } else if (this.rightKeyDown) { + this.steeringWheelPosition += wheelDelta; + } else { + this.steeringWheelPosition = 0.0F; + } + + if (this.steeringWheelPosition > 0.5) { + this.steeringWheelPosition = 0.5F; + } + + if (this.steeringWheelPosition < -0.5) { + this.steeringWheelPosition = -0.5F; + } + + this.DoPhysics(dt); + return true; + } + } + } + } + + protected void DoPhysics(float dt) { + System.out.println("-----------------------------"); + float engineTorque = this.vehicle.maxEngineTorque * this.acceleratorDepression * this.getTorque(this.engineRPM); + + assert this.vehicle.wheelDiameter != 0.0F; + + float gearRatio = this.getGearRatio(this.currentGear); + float wheelForce = engineTorque * this.vehicle.rearEndRatio * gearRatio / (this.vehicle.wheelDiameter * 0.5F); + if (Math.abs(this.velocityVectorCarFrame.y) > 2.0) { + wheelForce -= this.brakesDepression * this.vehicle.mass * 100.0F; + } else if (this.brakesDepression > 0.0F) { + this.velocityVectorCarFrame.y = 0.0F; + } + + if (gearRatio == 0.0F) { + wheelForce *= -1.0F; + } + + if (this.disabled) { + wheelForce = 0.0F; + } + + switch (this.vehicle.driveType) { + case 0: + wheelForce = (float)(wheelForce * 0.25); + this.tires[0].driveForce = this.tires[1].driveForce = wheelForce; + this.tires[2].driveForce = this.tires[3].driveForce = wheelForce; + break; + case 1: + wheelForce = (float)(wheelForce * 0.5); + this.tires[0].driveForce = this.tires[1].driveForce = wheelForce; + this.tires[2].driveForce = this.tires[3].driveForce = 0.0; + break; + case 2: + default: + wheelForce = (float)(wheelForce * 0.5); + this.tires[0].driveForce = this.tires[1].driveForce = 0.0; + this.tires[2].driveForce = this.tires[3].driveForce = wheelForce; + } + + float wheelAngle = this.steeringWheelPosition * 1.57F; + this.tires[0].wheelAngle = this.tires[1].wheelAngle = -wheelAngle; + float wheelRPM = 60.0F * this.velocityVectorCarFrame.y / ((float) Math.PI * this.vehicle.wheelDiameter); + this.engineRPM = wheelRPM * this.vehicle.rearEndRatio * gearRatio; + if (this.engineRPM < this.vehicle.idleRPM) { + this.engineRPM = this.vehicle.idleRPM; + } + + if (!this.vehicle.stickShift) { + if (this.engineRPM > this.vehicle.rpmTorquePeak && this.currentGear < 5) { + this.currentGear++; + } + + if (this.engineRPM < this.minRPMs[this.currentGear] && this.currentGear > 1) { + this.currentGear--; + } + } + + Point3Temp netForce = Point3Temp.make(0.0F, 0.0F, 0.0F); + Point3Temp netTorque = Point3Temp.make(0.0F, 0.0F, 0.0F); + + for (int wheel = 0; wheel < 4; wheel++) { + this.calculateForces(this.tires[wheel]); + netForce.plus(this.tires[wheel].force); + netTorque.plus(this.tires[wheel].torque); + } + + Point3Temp airFriction = Point3Temp.make(this.velocityVectorCarFrame); + airFriction.negate(); + airFriction.normalize(); + float drag = 0.15F * this.vehicle.frontalArea * 0.0801F * this.velocity * this.velocity / 32.1F; + airFriction.times(drag); + netForce.plus(airFriction); + netForce.vectorTimes(this.pilot.getObjectToWorldMatrix()); + System.out.println("Net force " + netForce.x + " " + netForce.y + " " + netForce.z); + System.out.println("Net torque " + netTorque.x + " " + netTorque.y + " " + netTorque.z); + Point3Temp gravityForce = Point3Temp.make(0.0F, 0.0F, -this.vehicle.mass * 32.1F); + netForce.plus(gravityForce); + Point3Temp inertialDampener = Point3Temp.make(0.5F, 0.5F, 0.5F); + inertialDampener.times(this.angularVelocityCarFrame); + inertialDampener.times(this.vehicle.momentsOfInertia); + inertialDampener.times(1.0F / dt); + netTorque.minus(inertialDampener); + this.integratedForce.plus(netForce); + this.integratedForce.times(0.5F); + this.integratedTorque.plus(netTorque); + this.integratedTorque.times(0.5F); + inertialDampener = Point3Temp.make(this.integratedForce); + inertialDampener.times(1.0F / this.vehicle.mass); + Point3Temp dV = Point3Temp.make(inertialDampener); + dV.times(dt); + Point3Temp lastV = Point3Temp.make(this.velocityVector); + Point3Temp lastW = Point3Temp.make(this.angularVelocityCarFrame); + this.velocityVector.plus(dV); + this.velocityVector.plus(lastV); + this.velocityVector.times(0.5F); + this.velocity = this.velocityVector.length(); + this.velocityVectorCarFrame.copy(this.velocityVector); + this.velocityVectorCarFrame.vectorTimes(this.pilot.getObjectToWorldMatrix().invert()); + Point3Temp dw = Point3Temp.make(this.integratedTorque); + dw.times(dt); + dw.dividedBy(this.vehicle.momentsOfInertia); + this.angularVelocityCarFrame.plus(dw); + this.angularVelocityCarFrame.plus(lastW); + this.angularVelocityCarFrame.times(0.5F); + this.angularVelocity.copy(this.angularVelocityCarFrame); + this.angularVelocity.vectorTimes(this.pilot.getObjectToWorldMatrix()); + Point3Temp dx = Point3Temp.make(this.velocityVector); + dx.times(dt); + float maxUnderground = 0.0F; + + for (int tire = 0; tire < 4; tire++) { + float diff = -this.tires[tire].underGround; + if (diff > maxUnderground) { + maxUnderground = diff; + } + } + + dx.z += maxUnderground; + dx.times(30.48F); + Point3Temp da = Point3Temp.make(this.angularVelocity); + da.times(dt); + this.pilot.premoveThrough(dx); + float convert = 180.0F / (float)Math.PI; + this.pilot.pitch(da.x * convert); + this.pilot.roll(da.y * convert); + this.pilot.yaw(da.z * convert); + System.out.println("dx " + dx.x + " " + dx.y + " " + dx.z); + System.out.println("da " + da.x + " " + da.y + " " + da.z); + System.out.println("pos" + this.pilot.getX() + " " + this.pilot.getY() + " " + this.pilot.getZ()); + } + + protected void calculateForces(VehicleDriver.Tire tire) { + tire.force.set(0.0F, 0.0F, 0.0F); + tire.torque.set(0.0F, 0.0F, 0.0F); + Transform o2w = this.pilot.getObjectToWorldMatrix(); + Transform w2o = this.pilot.getObjectToWorldMatrix().invert(); + Point3Temp worldPos = Point3Temp.make(tire.relativePos); + worldPos.plus(this.vehicle.centerOfGravity); + worldPos.times(30.48F); + worldPos.times(o2w); + double groundZ = this.room.floorHeight(worldPos.x, worldPos.y, worldPos.z); + Point3 N = this.room.surfaceNormal(worldPos.x, worldPos.y, worldPos.z); + groundZ *= 0.0328084F; + worldPos.times(0.0328084F); + tire.underGround = (float)(worldPos.z - groundZ); + if (!(tire.underGround > this.vehicle.shockLength)) { + Point3Temp R = Point3Temp.make(tire.relativePos); + R.vectorTimes(o2w); + Point3Temp w = Point3Temp.make(this.angularVelocity); + w.cross(R); + w.plus(this.velocityVector); + Point3Temp Vp = Point3Temp.make(w); + System.out.println("R " + R.x + " " + R.y + " " + R.z); + System.out.println("Vp " + Vp.x + " " + Vp.y + " " + Vp.z); + double normalSpeed = Vp.dot(N); + Point3Temp No = Point3Temp.make(N); + No.vectorTimes(w2o); + Point3Temp tmp = Point3Temp.make(No); + double scalarInertialMoment = tmp.dot(this.vehicle.momentsOfInertia); + scalarInertialMoment = Math.abs(scalarInertialMoment); + Point3Temp orthoNormal = Point3Temp.make(R); + orthoNormal.cross(N); + double r = orthoNormal.length(); + double denominator = r * this.vehicle.mass + scalarInertialMoment; + + assert denominator != 0.0; + + double myMass = this.vehicle.mass * 0.25; + double weight = scalarInertialMoment * myMass / denominator; + if (weight < 0.0) { + weight = 0.0; + } + + if (weight > this.vehicle.mass) { + weight = myMass; + } + + assert this.vehicle.shockLength != 0.0F; + + double k = weight * 32.1F / this.vehicle.shockLength; + double springLength = worldPos.z - (groundZ + this.vehicle.shockLength); + if (springLength < -this.vehicle.shockLength) { + springLength = -this.vehicle.shockLength; + } + + if (springLength > this.vehicle.shockLength) { + springLength = this.vehicle.shockLength; + } + + double groundForce = -k * springLength; + double term = k * weight; + + assert term >= 0.0; + + double damp = -Math.sqrt(term) * normalSpeed * this.vehicle.shockDampingCoeff; + groundForce += damp; + Point3Temp groundForceVector = Point3Temp.make(No); + groundForceVector.normalize(); + groundForceVector.times((float)groundForce); + tire.force.plus(groundForceVector); + Point3Temp VpCarFrame = Point3Temp.make(Vp); + VpCarFrame.vectorTimes(w2o); + Point3Temp normalVelocity = Point3Temp.make(VpCarFrame); + normalVelocity.z = 0.0F; + Point3Temp intendedForce = Point3Temp.make(0.0F, 0.0F, 0.0F); + Transform wheelXform = Transform.make(); + wheelXform.yaw(tire.wheelAngle * 229.1831F); + float mu; + if (normalVelocity.length() < 0.001F) { + mu = 0.0F; + } else if (Math.abs(No.x) > 0.4F) { + mu = 0.0F; + } else { + normalVelocity.normalize(); + Point3Temp wheelDirection = Point3Temp.make(0.0F, 1.0F, 0.0F); + wheelDirection.vectorTimes(wheelXform); + wheelDirection.normalize(); + float cosSlipAngle = normalVelocity.dot(wheelDirection); + System.out.println("wheel direction " + wheelDirection.x + " " + wheelDirection.y + " " + wheelDirection.z); + System.out.println("normal velocity " + normalVelocity.x + " " + normalVelocity.y + " " + normalVelocity.z); + System.out.println("cos of slip angle " + cosSlipAngle); + mu = this.getLateralForceCoef(cosSlipAngle, 0); + float crossZ = normalVelocity.x * wheelDirection.y - normalVelocity.y * wheelDirection.x; + if (crossZ > 0.0F) { + mu *= -1.0F; + } + } + + intendedForce.x = mu * (float)groundForce; + intendedForce.y = (float)tire.driveForce; + System.out.println("Lateral forces " + intendedForce.x + ", " + intendedForce.y); + float forceMag = intendedForce.length(); + float maxForce = (float)groundForce * this.vehicle.tireAdhesiveLimit; + if (forceMag > maxForce) { + intendedForce.normalize(); + intendedForce.times(maxForce); + tire.slipping = true; + } else { + tire.slipping = false; + } + + intendedForce.vectorTimes(wheelXform); + tire.force.plus(intendedForce); + Point3Temp rollingFriction = Point3Temp.make(0.0F, 0.0F, 0.0F); + float S = 1.0F; + float magV = Vp.length(); + rollingFriction.y = (float)groundForce * 0.001F * (5.7F + 0.036F * magV * 0.6818182F) * S; + if (this.velocityVectorCarFrame.y > 0.0F) { + rollingFriction.y *= -1.0F; + } + + rollingFriction.vectorTimes(wheelXform); + System.out.println("Rolling friction " + rollingFriction.x + " " + rollingFriction.y + " " + rollingFriction.z); + wheelXform.recycle(); + Point3Temp rCopy = Point3Temp.make(tire.relativePos); + rCopy.negate(); + tire.torque.copy(tire.force); + tire.torque.cross(rCopy); + System.out.println("Underground " + springLength + " damping " + damp); + System.out.println("Force " + tire.force.x + " " + tire.force.y + " " + tire.force.z); + System.out.println("Torque " + tire.torque.x + " " + tire.torque.y + " " + tire.torque.z); + System.out.println(); + } + } + + private float getGearRatio(int gearNumber) { + float gearRatio; + switch (gearNumber) { + case 0: + gearRatio = this.vehicle.gearRatio1; + break; + case 1: + gearRatio = this.vehicle.gearRatio1; + break; + case 2: + gearRatio = this.vehicle.gearRatio2; + break; + case 3: + gearRatio = this.vehicle.gearRatio3; + break; + case 4: + gearRatio = this.vehicle.gearRatio4; + break; + case 5: + gearRatio = this.vehicle.gearRatio5; + break; + default: + System.out.println("Illegal gear " + this.currentGear); + gearRatio = 1.0F; + } + + return gearRatio; + } + + protected float getLateralForceCoef(float cosSlipAngle, int terrainType) { + if (cosSlipAngle > 1.0) { + cosSlipAngle = 1.0F; + } + + if (cosSlipAngle < -1.0) { + cosSlipAngle = -1.0F; + } + + float index = (float)Math.ceil(Math.abs(cosSlipAngle) * 1000.0F / 1.0F); + return muTable[terrainType][(int)index]; + } + + protected void initLateralForceTable() { + if (!lateralForceTableInited) { + lateralForceTableInited = true; + muTable = new float[2][1001]; + + for (int type = 0; type < 2; type++) { + for (int i = 0; i <= 1000; i++) { + float degrees = (float)Math.acos(i / 1000.0F); + degrees = degrees * 360.0F / (float) (Math.PI * 2); + switch (type) { + case 0: + if (degrees > 45.0F) { + degrees = 45.0F; + } + + muTable[type][i] = -0.084496F + + 0.1241739F * degrees + - 0.003676377F * (float)Math.pow(degrees, 2.0) + - 5.5137E-6F * (float)Math.pow(degrees, 3.0) + + 6.461E-7F * (float)Math.pow(degrees, 4.0); + break; + case 1: + muTable[type][i] = 0.055731F + 0.069871F * degrees - 0.002175398F * degrees * degrees; + } + + if (muTable[type][i] < 0.0F) { + muTable[type][i] = 0.0F; + } + } + } + } + } + + float getTorque(float rpm) { + if (rpm > 7000.0F) { + rpm = 7000.0F; + } + + if (rpm < 1500.0F) { + rpm = 1500.0F; + } + + int index = (int)(rpm / 7000.0F); + return torqueTable[index]; + } + + void initTorqueTable() { + if (!torqueTableInited) { + torqueTableInited = true; + torqueTable = new float[501]; + + for (int i = 0; i <= 500; i++) { + float rpm = i / 500.0F * 7000.0F; + if (rpm < 1500.0F) { + rpm = 1500.0F; + } + + int closest = 0; + + for (int j = 0; j < 12; j++) { + float thisRpm = j * 500.0F + 1500.0F; + if (thisRpm >= rpm) { + closest = j; + break; + } + } + + if (closest == 0) { + torqueTable[i] = rawTorqueData[0] / 98.0F; + } else { + Point2 low = new Point2(); + Point2 high = new Point2(); + Point2 result = new Point2(); + low.x = (closest - 1) * 500.0F + 1500.0F; + low.y = rawTorqueData[closest - 1]; + high.x = low.x + 500.0F; + high.y = rawTorqueData[closest]; + float alpha = (rpm - low.x) / 500.0F; + result.x = low.x * alpha + high.x * (1.0F - alpha); + result.y = low.y * alpha + high.y * (1.0F - alpha); + torqueTable[i] = result.y / 98.0F; + } + } + } + } + + private void initRPMTable() { + this.minRPMs = new float[6]; + float[] maxVel = new float[5]; + + for (int x = 0; x < 5; x++) { + maxVel[x] = this.vehicle.rpmTorquePeak * (float) Math.PI * this.vehicle.wheelDiameter / (60.0F * this.vehicle.rearEndRatio * this.getGearRatio(x)); + } + + this.minRPMs[0] = 0.0F; + + for (int x = 1; x <= 5; x++) { + this.minRPMs[x] = 60.0F * maxVel[x - 1] * this.vehicle.rearEndRatio * this.getGearRatio(x) / ((float) Math.PI * this.vehicle.wheelDiameter); + } + } + + protected class Tire { + Point3 force = new Point3(); + Point3 torque = new Point3(); + Point3 relativePos = new Point3(); + float underGround; + double driveForce = 0.0; + float wheelAngle; + boolean slipping; + + public Tire() { + this.underGround = 0.0F; + this.wheelAngle = 0.0F; + this.slipping = false; + } + } +} |