path: root/NET/worlds/scape/VehicleDriver.java

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;
      }
   }
}