/// <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());
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);
}
}
if (m_flying)
{
// Slight PID correction
vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 0.06f);
//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);
}
public void UpdatePositionAndVelocity()
{
if (!m_isSelected)
{
if (_parent == null)
{
Vector3 pv = Vector3.Zero;
bool lastZeroFlag = _zeroFlag;
if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
tempPosition3.Dispose();
if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
tempTransform3.Dispose();
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
tempOrientation2.Dispose();
if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
tempAngularVelocity1.Dispose();
if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
tempLinearVelocity1.Dispose();
tempTransform3 = Body.getInterpolationWorldTransform();
tempPosition3 = tempTransform3.getOrigin(); // vec
tempOrientation2 = tempTransform3.getRotation(); // ori
tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel
tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel
_torque = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(),
tempAngularVelocity1.getZ());
Vector3 l_position = Vector3.Zero;
Quaternion l_orientation = Quaternion.Identity;
m_lastposition = _position;
m_lastorientation = _orientation;
l_position.X = tempPosition3.getX();
l_position.Y = tempPosition3.getY();
l_position.Z = tempPosition3.getZ();
l_orientation.X = tempOrientation2.getX();
l_orientation.Y = tempOrientation2.getY();
l_orientation.Z = tempOrientation2.getZ();
l_orientation.W = tempOrientation2.getW();
if (l_position.X > ((int)Constants.RegionSize - 0.05f) || l_position.X < 0f || l_position.Y > ((int)Constants.RegionSize - 0.05f) || l_position.Y < 0f)
{
//base.RaiseOutOfBounds(l_position);
if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
{
_position = l_position;
//_parent_scene.remActivePrim(this);
if (_parent == null)
base.RequestPhysicsterseUpdate();
return;
}
else
{
if (_parent == null)
base.RaiseOutOfBounds(l_position);
return;
}
}
if (l_position.Z < -200000f)
{
// This is so prim that get lost underground don't fall forever and suck up
//
// Sim resources and memory.
// Disables the prim's movement physics....
// It's a hack and will generate a console message if it fails.
//IsPhysical = false;
//if (_parent == null)
//base.RaiseOutOfBounds(_position);
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
_velocity.X = 0;
_velocity.Y = 0;
_velocity.Z = 0;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
if (_parent == null)
base.RequestPhysicsterseUpdate();
m_throttleUpdates = false;
// throttleCounter = 0;
_zeroFlag = true;
//outofBounds = true;
}
if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
&& (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
&& (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
&& (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
{
_zeroFlag = true;
m_throttleUpdates = false;
}
else
{
//m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
_zeroFlag = false;
}
if (_zeroFlag)
{
_velocity.X = 0.0f;
_velocity.Y = 0.0f;
_velocity.Z = 0.0f;
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
//_orientation.w = 0f;
//_orientation.X = 0f;
//_orientation.Y = 0f;
//_orientation.Z = 0f;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
if (!m_lastUpdateSent)
{
m_throttleUpdates = false;
// throttleCounter = 0;
m_rotationalVelocity = pv;
if (_parent == null)
base.RequestPhysicsterseUpdate();
m_lastUpdateSent = true;
}
}
else
{
if (lastZeroFlag != _zeroFlag)
{
if (_parent == null)
base.RequestPhysicsterseUpdate();
}
m_lastVelocity = _velocity;
_position = l_position;
_velocity.X = tempLinearVelocity1.getX();
_velocity.Y = tempLinearVelocity1.getY();
_velocity.Z = tempLinearVelocity1.getZ();
_acceleration = ((_velocity - m_lastVelocity) / 0.1f);
_acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
_velocity.Y - m_lastVelocity.Y / 0.1f,
_velocity.Z - m_lastVelocity.Z / 0.1f);
//m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
if (_velocity.ApproxEquals(pv, 0.5f))
{
m_rotationalVelocity = pv;
}
else
{
m_rotationalVelocity = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ());
}
//m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
_orientation.X = l_orientation.X;
_orientation.Y = l_orientation.Y;
_orientation.Z = l_orientation.Z;
_orientation.W = l_orientation.W;
m_lastUpdateSent = false;
//if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
//{
if (_parent == null)
base.RequestPhysicsterseUpdate();
// }
// else
// {
// throttleCounter++;
//}
}
m_lastposition = l_position;
if (forceenable)
{
Body.forceActivationState(1);
forceenable = false;
}
}
else
{
// Not a body.. so Make sure the client isn't interpolating
_velocity.X = 0;
_velocity.Y = 0;
_velocity.Z = 0;
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
_zeroFlag = true;
}
}
}
/// <summary>
/// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
/// </summary>
public void UpdatePositionAndVelocity()
{
if (Body == null)
return;
//int val = Environment.TickCount;
CheckIfStandingOnObject();
//m_log.DebugFormat("time:{0}", Environment.TickCount - val);
//IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody);
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
// no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
Vector3 vec = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
// kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
if (vec.X < -10.0f) vec.X = 0.0f;
if (vec.Y < -10.0f) vec.Y = 0.0f;
if (vec.X > (int)Constants.RegionSize + 10.2f) vec.X = (int)Constants.RegionSize + 10.2f;
if (vec.Y > (int)Constants.RegionSize + 10.2f) vec.Y = (int)Constants.RegionSize + 10.2f;
m_position.X = vec.X;
m_position.Y = vec.Y;
m_position.Z = vec.Z;
// Did we move last? = zeroflag
// This helps keep us from sliding all over
if (m_zeroFlag)
{
m_velocity.X = 0.0f;
m_velocity.Y = 0.0f;
m_velocity.Z = 0.0f;
// Did we send out the 'stopped' message?
if (!m_lastUpdateSent)
{
m_lastUpdateSent = true;
base.RequestPhysicsterseUpdate();
}
}
else
{
m_lastUpdateSent = false;
vec = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
m_velocity.X = (vec.X);
m_velocity.Y = (vec.Y);
m_velocity.Z = (vec.Z);
//m_log.Debug(m_target_velocity);
if (m_velocity.Z < -6 && !m_hackSentFall)
{
m_hackSentFall = true;
m_pidControllerActive = false;
}
else if (m_flying && !m_hackSentFly)
{
//m_hackSentFly = true;
//base.SendCollisionUpdate(new CollisionEventUpdate());
}
else
{
m_hackSentFly = false;
m_hackSentFall = false;
}
}
if (Body != null)
{
if (Body.getFriction() < 0.9f)
Body.setFriction(0.9f);
}
//if (Body != null)
// Body.clearForces();
}
/// <summary>
/// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
/// </summary>
public void UpdatePositionAndVelocity()
{
if (Body == null)
return;
//int val = Environment.TickCount;
CheckIfStandingOnObject();
//m_log.DebugFormat("time:{0}", Environment.TickCount - val);
//IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody);
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
// no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
Vector3 vec = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
// kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
if (vec.X < -10.0f) vec.X = 0.0f;
if (vec.Y < -10.0f) vec.Y = 0.0f;
if (vec.X > m_parent_scene.m_region.RegionSizeX + 10.2f) vec.X = m_parent_scene.m_region.RegionSizeX + 10.2f;
if (vec.Y > m_parent_scene.m_region.RegionSizeY + 10.2f) vec.Y = m_parent_scene.m_region.RegionSizeY + 10.2f;
m_position.X = vec.X;
m_position.Y = vec.Y;
m_position.Z = vec.Z;
// Did we move last? = zeroflag
// This helps keep us from sliding all over
if (m_zeroFlag)
{
m_velocity.X = 0.0f;
m_velocity.Y = 0.0f;
m_velocity.Z = 0.0f;
// Did we send out the 'stopped' message?
if (!m_lastUpdateSent)
{
m_lastUpdateSent = true;
base.RequestPhysicsterseUpdate();
}
//Tell any listeners that we've stopped
base.TriggerMovementUpdate();
}
else
{
m_lastUpdateSent = false;
vec = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
m_velocity.X = (vec.X);
m_velocity.Y = (vec.Y);
m_velocity.Z = (vec.Z);
//m_log.Debug(m_target_velocity);
if (m_velocity.Z < -6 && !m_hackSentFall)
{
m_hackSentFall = true;
m_pidControllerActive = false;
}
else if (m_flying && !m_hackSentFly)
{
//m_hackSentFly = true;
//base.SendCollisionUpdate(new CollisionEventUpdate());
}
else
{
m_hackSentFly = false;
m_hackSentFall = false;
}
const float VELOCITY_TOLERANCE = 0.001f;
const float POSITION_TOLERANCE = 0.05f;
//Check to see whether we need to trigger the significant movement method in the presence
if (!RotationalVelocity.ApproxEquals(m_lastRotationalVelocity, VELOCITY_TOLERANCE) ||
!Velocity.ApproxEquals(m_lastVelocity, VELOCITY_TOLERANCE) ||
!Position.ApproxEquals(m_lastPosition, POSITION_TOLERANCE))
{
// Update the "last" values
m_lastPosition = Position;
m_lastRotationalVelocity = RotationalVelocity;
m_lastVelocity = Velocity;
base.RequestPhysicsterseUpdate();
base.TriggerSignificantMovement();
}
//Tell any listeners about the new info
base.TriggerMovementUpdate();
}
if (Body != null)
{
if (Body.getFriction() < 0.9f)
Body.setFriction(0.9f);
}
//if (Body != null)
// Body.clearForces();
}
