private void IntegratePosition(float delta_t)
{
Vector earth_velocity;
velocity.IncrementX(delta_t * acceleration.X);
velocity.IncrementY(delta_t * acceleration.Y);
velocity.IncrementZ(delta_t * acceleration.Z);
if (velocity.X > 48.0)
{
velocity.X = 48.0;
acceleration.X = -1.0;
}
if (velocity.X < 0.0)
{
velocity.X = 0.0;
acceleration.X = 0.5;
}
attitude = attitude + (attitude_rate * delta_t);
attitude.Limits();
earth_velocity = new Vector(velocity);
attitude.RotateAtoE(earth_velocity);
position.IncrementX(delta_t * earth_velocity.X);
position.IncrementY(delta_t * earth_velocity.Y);
position.IncrementZ(delta_t * earth_velocity.Z);
// Debug.WriteLine("accelZ = " + acceleration.Z + " velz = " + velocity.Z + " posz = " + position.Z);
mph = (float)velocity.X * 3600.0f / 5280.0f;
}
} // end Limits
///////////////////////////////////////////////////////////////////////////////
public void Limits(
Euler results
)
{
// Flip heading and roll when we go over the top or through the bottom.
if (results.Theta > (Math.PI / 2.0))
{
theta = (float)Math.PI - results.Theta;
psi = (float)AEPCPI(results.Psi + (float)Math.PI);
phi = (float)AEPCPI(results.Phi + (float)Math.PI);
}
else if (results.Theta < -(Math.PI / 2.0))
{
theta = -(float)Math.PI - results.Theta;
psi = (float)AEPCPI(results.Psi + (float)Math.PI);
phi = (float)AEPCPI(results.Phi + (float)Math.PI);
}
else
{
theta = results.Theta;
psi = (float)AEPCPI(results.Psi);
phi = (float)AEPCPI(results.Phi);
}
} // end Limits
public void Process(float delta_t, Euler attitude, Vector acceleration, Vector velocity, Vector position)
{
double temp;
double susp_delta;
double squeel_force;
double slide_force;
double grab_force;
double tire_side_force;
earth_location.X = offset.X;
earth_location.Y = offset.Y;
earth_location.Z = offset.Z + suspension_offset;
attitude.RotateAtoE(earth_location);
altitude = position.Z + earth_location.Z - radius;
height_above_ground = altitude - ground_height;
touching_ground = height_above_ground <= 0.0f;
if (touching_ground)
{
suspension_offset = -height_above_ground;
}
else
{
suspension_offset = -max_suspension_offset;
}
susp_delta = (suspension_offset + height_above_ground) * delta_t;
if (Math.Abs(upwards_force - weight_over_wheel) < 2.0)
{
suspension_offset -= susp_delta;
}
if (suspension_offset > max_suspension_offset)
{
suspension_offset = max_suspension_offset;
}
else if (suspension_offset < -max_suspension_offset)
{
suspension_offset = -max_suspension_offset;
}
bottomed_out = suspension_offset == max_suspension_offset;
temp = (0.3f * (suspension_offset * suspension_offset) / (max_suspension_offset * max_suspension_offset));
if (suspension_offset < 0.0f)
{
temp *= -1.0f;
}
if (Math.Abs(suspension_offset) < 0.3f)
{
temp = 0.0f; // suspension neutral
}
temp += 1.0f;
if (!touching_ground)
{
temp = 0.0f;
}
double spring_force = spring_constant * suspension_offset;
double damping_force = damping * suspension_offset / max_suspension_offset;
if (touching_ground)
{
upwards_force = (spring_force + damping_force);
}
else
{
upwards_force = 0.0;
}
// upwards_force = static_weight_over_wheel * temp;
if ((upwards_force - weight_over_wheel) > 2.0f)
{
suspension_offset -= 0.5f * delta_t;
}
else if ((upwards_force - weight_over_wheel) < -2.0f)
{
suspension_offset += 0.5f * delta_t;
}
slide_force = 32.0f * stiction * friction;
squeel_force = 0.9f * slide_force;
grab_force = 32.0f * sliding_friction;
if ((acceleration.Y > 0.0f && rel_heading > 0.0f) || (acceleration.Y < 0.0f && rel_heading < 0.0f))
{
tire_side_force = (float)Math.Abs(acceleration.Y * (1.0f - Math.Cos(rel_heading)));
}
else
{
tire_side_force = (float)Math.Abs(acceleration.Y * Math.Cos(rel_heading));
}
squealing = false;
sliding = false;
if (drive_wheel && acceleration.X >= slide_force)
{
sliding = true;
}
if ((acceleration.X < -squeel_force && acceleration.X > -slide_force) ||
(tire_side_force > squeel_force && tire_side_force < slide_force))
{
squealing = true;
}
if (acceleration.X <= -slide_force || tire_side_force >= slide_force)
{
sliding = true;
}
if (Math.Abs(acceleration.X) < grab_force && Math.Abs(velocity.Y) < grab_force && tire_side_force < grab_force)
{
sliding = false;
}
}
public double ABSDeltaPhi(
Euler from
)
{
return(Math.Abs(phi - from.phi));
}
public double ABSDeltaTheta(
Euler from
)
{
return(Math.Abs(theta - from.theta));
}
public double ABSDeltaPsi(
Euler from
)
{
return(Math.Abs(psi - from.psi));
}
public Euler ABSDelta(
Euler from
)
{
return(new Euler(Math.Abs(psi - from.psi), Math.Abs(theta - from.theta), Math.Abs(phi - from.phi)));
}
public double DeltaPhi(
Euler from
)
{
return(phi - from.phi);
}
public double DeltaTheta(
Euler from
)
{
return(theta - from.theta);
}
public double DeltaPsi(
Euler from
)
{
return(psi - from.psi);
}
// Calculates the difference between two copies of the class.
public Euler Delta(
Euler from
)
{
return(new Euler(psi - from.psi, theta - from.theta, phi - from.phi));
}
// A copy constructor
public Euler(
Euler x_angle
)
{
psi = x_angle.Psi; theta = x_angle.Theta; phi = x_angle.Phi;
}