void FixedUpdate()
{
//Default direction
var forceDirection = transform.forward;
var steer = 0;
//steer direction
if (Input.GetKey(KeyCode.A))
{
steer = 1;
}
if (Input.GetKey(KeyCode.D))
{
steer = -1;
}
//Rotational force
rb.AddForceAtPosition(steer * transform.right * steerPower / 100f, Motor.position);
//compute vectors - in case the model is facing a different direction, force is applies proportionally still.
var forward = Vector3.Scale(new Vector3(1, 0, 1), transform.forward);
var targetVelocity = Vector3.zero;
//forward/backward power
if (Input.GetKey(KeyCode.W))
{
PhysicsHelper.ApplyForceToReachVelocity(rb, forward * maxSpeed, power);
}
if (Input.GetKey(KeyCode.S))
{
PhysicsHelper.ApplyForceToReachVelocity(rb, forward * -maxSpeed, power);
}
//Motor animation and particle system
Motor.SetPositionAndRotation(Motor.position, transform.rotation * startRotation * Quaternion.Euler(0, 30f * steer, 0));
if (particleSystem != null)
{
if (Input.GetKey(KeyCode.W) || Input.GetKey(KeyCode.S))
{
particleSystem.Play();
}
else
{
particleSystem.Pause;
}
}
}
void FixedUpdate()
{
if (!warmUp)
{
Profiler.BeginSample("PID Controller");
var rcCommand = Inputs.RCInput;
var inclination = Inputs.Angle;
var gyroADC = Inputs.Gyro;
var cycleTime = Inputs.CycleTime;
PidController.GetPID(axisPID, inclination, rcCommand, gyroADC, cycleTime);
float throttle = Inputs.RCInput [Constants.THROTTLE];
float[] newMotorSpeeds = new float[mixer.GetLength(0)];
for (int i = 0; i < newMotorSpeeds.Length; i++)
{
newMotorSpeeds [i] = throttle > 0 ? ((throttle * mixer [i, 0] + axisPID [Constants.PITCH] * mixer [i, 2] + axisPID [Constants.ROLL] * mixer [i, 1] - axisPID [Constants.YAW] * mixer [i, 3])) : 0f;
}
float overMax = 0f;
for (int i = 0; i < newMotorSpeeds.Length; i++)
{
overMax = Mathf.Max(overMax, newMotorSpeeds [i] - MAX_OUTPUT);
}
Profiler.EndSample();
Profiler.BeginSample("Multirotor Physics");
totalCurrent = 0f;
torque = 0f;
for (int i = 0; i < newMotorSpeeds.Length; i++)
{
float motorTorque;
if (((brokenProp >> i) & 1) == 0)
{
var motorVelocityGlobal = rigidBody.velocity;
Vector3 motorVelocity = transform.InverseTransformDirection(motorVelocityGlobal);
var x = motorVelocity [Constants.YAW];
var v = Mathf.Min(Mathf.Max(newMotorSpeeds [i] - overMax, 0f) / MAX_OUTPUT * y, maxV);
newMotorSpeeds [i] = Mathf.Max((Mathf.Sqrt(Mathf.Pow(x * K1 + C4, 2f) - K2 * (C1 - C2 * v - motorSpeeds [i] * C3)) + x * K3 - K4) / K5, 0f);
forces [i] = Mathf.Max(0f, newMotorSpeeds [i] * K8 * (K9 * newMotorSpeeds [i] - x));
motorTorque = (forces [i] * K7) + (newMotorSpeeds [i] - motorSpeeds [i]) * K6;
motorSpeeds [i] = newMotorSpeeds [i];
if (motorSpeeds [i] != 0)
{
Props [i].transform.Rotate(new Vector3(0f, 0f, -mixer [i, 3] * motorSpeeds [i] * Time.fixedDeltaTime / 60f * 360f));
}
if (forces [i] > 0)
{
RigidBody.AddForceAtPosition(transform.TransformDirection(new Vector3(0f, forces [i] * scale, 0f)), (Props [i].transform.position));
}
var current = (v - motorSpeeds [i] / z) / r;
totalCurrent += Math.Max(current, 0f);
}
else
{
motorTorque = -motorSpeeds [i] * K6;
motorSpeeds [i] = 0f;
}
if (motorTorque != 0f)
{
addTorqueAtPosition(transform.TransformDirection(new Vector3(0f, mixer [i, 3] * motorTorque * scale, 0f)), Props [i].transform.position, rigidBody);
}
}
if ((brokenProp != 0 || isCollided > 0 && Vector3.Dot(transform.up, Vector3.up) < 0) && RigidBody.IsSleeping())
{
brokenProp = 0;
Reset();
}
maxV = (y + x1 * totalCurrent);
var levelDirection = transform.position - levelCenter.transform.position;
var levelDistance = (levelDirection).magnitude;
if (levelDistance > 600f)
{
transform.position += levelDirection / levelDistance * (600f - levelDistance);
}
Profiler.EndSample();
}
}
