/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move(float timeStep)
{
// no lock; for now it's only called from within Simulate()
// If the PID Controller isn't active then we set our force
// calculating base velocity to the current position
if (Body == null)
{
return;
}
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
if (m_pidControllerActive == false)
{
m_zeroPosition.X = tempVector1.getX();
m_zeroPosition.Y = tempVector1.getY();
m_zeroPosition.Z = tempVector1.getZ();
}
//PidStatus = true;
Vector3 vec = Vector3.Zero;
Vector3 vel = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
vel *= 0.25f;
float movementdivisor = 1f;
if (!m_alwaysRun)
{
movementdivisor = walkDivisor;
}
else
{
movementdivisor = runDivisor;
}
// if velocity is zero, use position control; otherwise, velocity control
if (m_target_velocity.X == 0.0f && m_target_velocity.Y == 0.0f && m_target_velocity.Z == 0.0f && m_iscolliding)
{
// keep track of where we stopped. No more slippin' & slidin'
if (!m_zeroFlag)
{
m_zeroFlag = true;
m_zeroPosition.X = tempVector1.getX();
m_zeroPosition.Y = tempVector1.getY();
m_zeroPosition.Z = tempVector1.getZ();
}
if (m_pidControllerActive)
{
// We only want to deactivate the PID Controller if we think we want to have our surrogate
// react to the physics scene by moving it's position.
// Avatar to Avatar collisions
// Prim to avatar collisions
Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
vec.X = (m_target_velocity.X - vel.X) * (PID_D) + (m_zeroPosition.X - pos.X) * (PID_P * 2);
vec.Y = (m_target_velocity.Y - vel.Y) * (PID_D) + (m_zeroPosition.Y - pos.Y) * (PID_P * 2);
if (m_flying)
{
vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) + (m_zeroPosition.Z - pos.Z) * PID_P;
}
}
//PidStatus = true;
}
else
{
m_pidControllerActive = true;
m_zeroFlag = false;
if (m_iscolliding && !m_flying)
{
// We're standing on something
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
}
else if (m_iscolliding && m_flying)
{
// We're flying and colliding with something
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16);
}
else if (!m_iscolliding && m_flying)
{
// we're in mid air suspended
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6);
// We don't want linear velocity to cause our avatar to bounce, so we check target Z and actual velocity X, Y
// rebound preventing
//if (m_target_velocity.Z < 0.025f && m_velocity.X < 0.25f && m_velocity.Y < 0.25f)
// m_zeroFlag = true;
}
if (m_iscolliding && !m_flying && m_target_velocity.Z > 0.0f)
{
// We're colliding with something and we're not flying but we're moving
// This means we're walking or running.
Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
vec.Z = (m_target_velocity.Z - vel.Z) * PID_D + (m_zeroPosition.Z - pos.Z) * PID_P;
if (m_target_velocity.X > 0)
{
vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D;
}
if (m_target_velocity.Y > 0)
{
vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
}
}
else if (!m_iscolliding && !m_flying)
{
// we're not colliding and we're not flying so that means we're falling!
// m_iscolliding includes collisions with the ground.
// d.Vector3 pos = d.BodyGetPosition(Body);
if (m_target_velocity.X > 0)
{
vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D;
}
if (m_target_velocity.Y > 0)
{
vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
}
}
if (m_flying)
{
vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) * 10;
}
}
if (m_flying)
{
// Slight PID correction
vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 1.5f);
//auto fly height. Kitto Flora
//d.Vector3 pos = d.BodyGetPosition(Body);
float target_altitude = m_parent_scene.GetTerrainHeightAtXY(m_position.X, m_position.Y) + m_parent_scene.minimumGroundFlightOffset;
if (m_position.Z < target_altitude)
{
vec.Z += (target_altitude - m_position.Z) * PID_P * 5.0f;
}
}
if (Body != null && (((m_target_velocity.X > 0.2f || m_target_velocity.X < -0.2f) || (m_target_velocity.Y > 0.2f || m_target_velocity.Y < -0.2f))))
{
Body.setFriction(0.001f);
//m_log.DebugFormat("[PHYSICS]: Avatar force applied: {0}, Target:{1}", vec.ToString(), m_target_velocity.ToString());
}
if (Body != null)
{
int activationstate = Body.getActivationState();
if (activationstate == 0)
{
Body.forceActivationState(1);
}
}
doImpulse(vec, true);
}