/// <summary> /// Updates the rotational state etc /// </summary> /// <param name="dt"></param> public void Update(float dt) { if (dt <= 0.0f) { return; } float origAngVel = angVel; upSpeed = (displacement - lastDisplacement) / System.Math.Max(dt, JiggleMath.Epsilon); if (locked) { angVel = 0; torque = 0; } else { angVel += torque * dt / inertia; torque = 0; // prevent friction from reversing dir - todo do this better // by limiting the torque if (((origAngVel > angVelForGrip) && (angVel < angVelForGrip)) || ((origAngVel < angVelForGrip) && (angVel > angVelForGrip))) { angVel = angVelForGrip; } angVel += driveTorque * dt / inertia; driveTorque = 0; float maxAngVel = 200; angVel = OpenTKHelper.Clamp(angVel, -maxAngVel, maxAngVel); axisAngle += OpenTKHelper.ToDegrees(dt * angVel); } }
/// <summary> /// Update stuff at the end of physics /// </summary> /// <param name="dt"></param> public void PostPhysics(float dt) { for (int i = 0; i < wheels.Count; i++) { wheels[i].Update(dt); } // control inputs float deltaAccelerate = dt * 4.0f; float deltaSteering = dt * steerRate; // update the actual values float dAccelerate = destAccelerate - accelerate; dAccelerate = OpenTKHelper.Clamp(dAccelerate, -deltaAccelerate, deltaAccelerate); accelerate += dAccelerate; float dSteering = destSteering - steering; dSteering = OpenTKHelper.Clamp(dSteering, -deltaSteering, deltaSteering); steering += dSteering; // apply these inputs float maxTorque = driveTorque; if (fWDrive && bWDrive) { maxTorque *= 0.5f; } if (fWDrive) { wheels[(int)WheelId.WheelFL].AddTorque(maxTorque * accelerate); wheels[(int)WheelId.WheelFR].AddTorque(maxTorque * accelerate); } if (bWDrive) { wheels[(int)WheelId.WheelBL].AddTorque(maxTorque * accelerate); wheels[(int)WheelId.WheelBR].AddTorque(maxTorque * accelerate); } wheels[(int)WheelId.WheelBL].Lock = (hBrake > 0.5f); wheels[(int)WheelId.WheelBR].Lock = (hBrake > 0.5f); // steering angle applies to the inner wheel. The outer one needs to match it int inner, outer; if (steering > 0.0f) { inner = (int)WheelId.WheelFL; outer = (int)WheelId.WheelFR; } else { inner = (int)WheelId.WheelFR; outer = (int)WheelId.WheelFL; } float alpha = System.Math.Abs(maxSteerAngle * steering); float angleSgn = steering > 0.0f ? 1.0f : -1.0f; wheels[inner].SteerAngle = (angleSgn * alpha); float beta; if (alpha == 0.0f) { beta = alpha; } else { float dx = (wheels[(int)WheelId.WheelFR].Pos.X - wheels[(int)WheelId.WheelBR].Pos.X); float dy = (wheels[(int)WheelId.WheelFL].Pos.Z - wheels[(int)WheelId.WheelFR].Pos.Z); //beta = ATan2Deg(dy, dx + (dy / TanDeg(alpha))); beta = (float)System.Math.Atan2(OpenTKHelper.ToRadians(dy), OpenTKHelper.ToRadians(dx + (dy / (float)System.Math.Tan(OpenTKHelper.ToRadians(alpha))))); beta = OpenTKHelper.ToDegrees(beta); } wheels[outer].SteerAngle = (angleSgn * beta); }