/// <summary>
/// Adds the force supplied to the Target Velocity
/// The PID controller takes this target velocity and tries to make it a reality
/// </summary>
/// <param name="force"></param>
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
if (pushforce)
{
AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f));
}
else
{
AddChange(changes.Velocity, force);
}
}
else
{
m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
}
//m_lastUpdateSent = false;
}
private void SetTaintedCapsuleLength(Vector3 size)
{
if (size.IsFinite())
{
m_pidControllerActive = true;
m_tainted_CAPSULE_LENGTH = size.Z - CAPSULE_RADIUS * 2.0f;
// m_log.InfoFormat("[ODE CHARACTER]: Size = {0}, Capsule Length = {1} (Capsule Radius = {2})",
// size, m_tainted_CAPSULE_LENGTH, CAPSULE_RADIUS);
}
else
{
m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Size for {0} in {1}", Name, _parent_scene.Name);
}
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="defects">The character will be added to this list if there is something wrong (non-finite
/// position or velocity).
/// </param>
internal void Move(List<OdeCharacter> defects)
{
// 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 == IntPtr.Zero)
return;
if (m_pidControllerActive == false)
{
_zeroPosition = d.BodyGetPosition(Body);
}
//PidStatus = true;
d.Vector3 localpos = d.BodyGetPosition(Body);
Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
if (!localPos.IsFinite())
{
m_log.WarnFormat(
"[ODE CHARACTER]: Avatar position of {0} for {1} is non-finite! Removing from physics scene.",
localPos, Name);
defects.Add(this);
return;
}
Vector3 vec = Vector3.Zero;
d.Vector3 vel = d.BodyGetLinearVel(Body);
// m_log.DebugFormat(
// "[ODE CHARACTER]: Current velocity in Move() is <{0},{1},{2}>, target {3} for {4}",
// vel.X, vel.Y, vel.Z, _target_velocity, Name);
float movementdivisor = 1f;
if (!m_alwaysRun)
{
movementdivisor = walkDivisor;
}
else
{
movementdivisor = runDivisor;
}
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
{
// keep track of where we stopped. No more slippin' & slidin'
if (!_zeroFlag)
{
_zeroFlag = true;
_zeroPosition = d.BodyGetPosition(Body);
}
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
d.Vector3 pos = d.BodyGetPosition(Body);
vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
if (flying)
{
vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
}
}
//PidStatus = true;
}
else
{
m_pidControllerActive = true;
_zeroFlag = false;
if (m_iscolliding && !flying)
{
// We're standing on something
vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
}
else if (m_iscolliding && flying)
{
// We're flying and colliding with something
vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16);
vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16);
}
else if (!m_iscolliding && flying)
{
// we're in mid air suspended
vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6);
vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6);
// m_log.DebugFormat(
// "[ODE CHARACTER]: !m_iscolliding && flying, vec {0}, _target_velocity {1}, movementdivisor {2}, vel {3}",
// vec, _target_velocity, movementdivisor, vel);
}
if (flying)
{
// This also acts as anti-gravity so that we hover when flying rather than fall.
vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
}
else
{
if (m_iscolliding && _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.
d.Vector3 pos = d.BodyGetPosition(Body);
vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D;
vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
}
else if (!m_iscolliding)
{
// we're not colliding and we're not flying so that means we're falling!
// m_iscolliding includes collisions with the ground.
vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D;
vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
}
}
}
if (flying)
{
// Anti-gravity so that we hover when flying rather than fall.
vec.Z += ((-1 * _parent_scene.gravityz) * m_mass);
//Added for auto fly height. Kitto Flora
//d.Vector3 pos = d.BodyGetPosition(Body);
float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
if (_position.Z < target_altitude)
{
vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
}
// end add Kitto Flora
}
if (vec.IsFinite())
{
// Apply the total force acting on this avatar
d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
if (!_zeroFlag)
AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
}
else
{
m_log.WarnFormat(
"[ODE CHARACTER]: Got a NaN force vector {0} in Move() for {1}. Removing character from physics scene.",
vec, Name);
defects.Add(this);
return;
}
d.Vector3 newVel = d.BodyGetLinearVel(Body);
if (newVel.X >= 256 || newVel.X <= 256 || newVel.Y >= 256 || newVel.Y <= 256 || newVel.Z >= 256 || newVel.Z <= 256)
{
// m_log.DebugFormat(
// "[ODE CHARACTER]: Limiting falling velocity from {0} to {1} for {2}", newVel.Z, -9.8, Name);
newVel.X = Util.Clamp<float>(newVel.X, -255f, 255f);
newVel.Y = Util.Clamp<float>(newVel.Y, -255f, 255f);
if (!flying)
newVel.Z
= Util.Clamp<float>(
newVel.Z, -_parent_scene.AvatarTerminalVelocity, _parent_scene.AvatarTerminalVelocity);
else
newVel.Z = Util.Clamp<float>(newVel.Z, -255f, 255f);
d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
}
}
private void SetTaintedCapsuleLength(Vector3 size)
{
if (size.IsFinite())
{
m_pidControllerActive = true;
m_tainted_CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
// m_log.Info("[ODE CHARACTER]: " + CAPSULE_LENGTH);
}
else
{
m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Size for {0} in {1}", Name, _parent_scene.Name);
}
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
/// <returns>True if the avatar should be removed from the simulation</returns>
public void Move(float timeStep)
{
if (Body == IntPtr.Zero || !IsPhysical)
return;
Vector3 vec = Vector3.Zero;
Vector3 vel = d.BodyGetLinearVel(Body).ToVector3();
Vector3 tempPos = d.BodyGetPosition(Body).ToVector3();
#region Flight Ceiling
// rex, added height check
if (_parent_scene.m_useFlightCeilingHeight && tempPos.Z > _parent_scene.m_flightCeilingHeight)
{
tempPos.Z = _parent_scene.m_flightCeilingHeight;
d.BodySetPosition(Body, tempPos.X, tempPos.Y, tempPos.Z);
if (vel.Z > 0.0f)
{
vel.Z = 0.0f;
d.BodySetLinearVel(Body, vel.X, vel.Y, vel.Z);
}
if (m_targetVelocity.Z > 0.0f)
m_targetVelocity.Z = 0.0f;
}
// endrex
#endregion
#region NonFinite Pos
Vector3 localPos = new Vector3(tempPos.X, tempPos.Y, tempPos.Z);
if (!localPos.IsFinite())
{
MainConsole.Instance.Warn("[ODE Physics]: Avatar Position is non-finite!");
_parent_scene.BadCharacter(this);
return;
}
#endregion
#region Check for out of region
if (Position.X < 0.25f || Position.Y < 0.25f ||
Position.X > _parent_scene.Region.RegionSizeX - .25f ||
Position.Y > _parent_scene.Region.RegionSizeY - .25f)
{
if (!CheckForRegionCrossing())
{
Vector3 newPos = Position;
newPos.X = Util.Clip(Position.X, 0.75f, _parent_scene.Region.RegionSizeX - 0.75f);
newPos.Y = Util.Clip(Position.Y, 0.75f, _parent_scene.Region.RegionSizeY - 0.75f);
Position = newPos;
d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
}
}
#endregion
#region Movement Multiplier
float movementmult = 1f;
if (!m_alwaysRun)
movementmult /= _parent_scene.avMovementDivisorWalk;
else
movementmult /= _parent_scene.avMovementDivisorRun;
movementmult *= 10;
movementmult *= SpeedModifier;
if (flying)
movementmult *= _parent_scene.m_AvFlySpeed;
#endregion
#region Jump code
if (IsJumping)
{
if (flying ||
((IsColliding) && m_preJumpCounter > _parent_scene.m_preJumpTime || m_preJumpCounter > 150))
{
m_isJumping = false;
m_preJumpCounter = 0;
m_targetVelocity.X /= 2;
m_targetVelocity.Y /= 2;
m_targetVelocity.Z = -0.5f;
}
else
{
m_targetVelocity.X = m_preJumpForce.X * _parent_scene.m_preJumpForceMultiplierX / 2.5f;
m_targetVelocity.Y = m_preJumpForce.Y * _parent_scene.m_preJumpForceMultiplierY / 2.5f;
m_preJumpCounter++;
}
}
else if (m_ispreJumping)
{
if (m_preJumpCounter == _parent_scene.m_preJumpTime)
{
m_ispreJumping = false;
m_targetVelocity.X = m_preJumpForce.X;
m_targetVelocity.Y = m_preJumpForce.Y;
m_targetVelocity.Z = m_preJumpForce.Z *
(m_alwaysRun
? _parent_scene.m_preJumpForceMultiplierZ/2.5f
: _parent_scene.m_preJumpForceMultiplierZ/2.25f);
m_preJumpCounter = 0;
m_isJumping = true;
}
else
{
m_preJumpCounter++;
TriggerMovementUpdate();
return;
}
}
#endregion
#region Check for underground
d.AABB aabb;
d.GeomGetAABB(Shell, out aabb);
float chrminZ = aabb.MinZ;
Vector3 posch = localPos;
float ftmp;
if (flying)
{
ftmp = 0.75f*timeStep;
posch.X += vel.X*ftmp;
posch.Y += vel.Y*ftmp;
}
float groundHeight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
if (chrminZ < groundHeight)
{
float depth = groundHeight - chrminZ;
if (m_targetVelocity.Z < 0)
m_targetVelocity.Z = 0;
if (!flying)
{
if (vel.Z < -10f)
vel.Z = -10f;
vec.Z = -vel.Z*PID_D*1.5f + depth*PID_P*50.0f;
}
else
{
vec.Z = depth*PID_P*50.0f;
}
if (depth < 0.12f)
{
if (!m_iscolliding)
{
m_iscolliding = true;
m_colliderfilter = 15;
ContactPoint point = new ContactPoint
{
Type = ActorTypes.Ground,
PenetrationDepth = depth,
Position = {X = localPos.X, Y = localPos.Y, Z = chrminZ},
SurfaceNormal = new Vector3(0, 0, -1f)
};
//0 is the ground localID
AddCollisionEvent(0, point);
}
vec.Z *= 0.5f;
}
}
/*
if(Flying && _target_velocity == Vector3.Zero &&
Math.Abs(vel.Z) < 0.1)
notMoving = true;
*/
#endregion
#region Gravity
if (!flying)
vec.Z += -9.8f * 35 * Mass
* ( _appliedFallingForce > 100 ? 1 : _appliedFallingForce++/100f)
* (IsTruelyColliding ? 0.5f : 1.0f);
else if (_parent_scene.AllowAvGravity && m_targetVelocity.Z > 0 &&
tempPos.Z > _parent_scene.AvGravityHeight) //Should be stop avies from flying upwards
{
//How much should we force them down?
float Multiplier = (_parent_scene.AllowAvsToEscapeGravity ? .03f : .1f);
//How much should we force them down?
float fudgeHeight = (_parent_scene.AllowAvsToEscapeGravity ? 80 : 30);
//We add the 30 so that gravity is resonably strong once they pass the min height
Multiplier *= tempPos.Z + fudgeHeight - _parent_scene.AvGravityHeight;
//Limit these so that things don't go wrong
if (Multiplier < 1)
Multiplier = 1;
float maxpower = (_parent_scene.AllowAvsToEscapeGravity ? 1.5f : 3f);
if (Multiplier > maxpower)
Multiplier = maxpower;
m_targetVelocity.Z /= Multiplier;
vel.Z /= Multiplier;
}
if (IsColliding)
_appliedFallingForce = 10;
#endregion
#region Auto Fly Height
if (Flying)
{
//Added for auto fly height. Kitto Flora
//Changed to only check if the avatar is flying around,
// Revolution: If the avatar is going down, they are trying to land (probably), so don't push them up to make it harder
// Only if they are moving around sideways do we need to push them up
if (m_targetVelocity.X != 0 || m_targetVelocity.Y != 0)
{
Vector3 forwardVel = new Vector3(m_targetVelocity.X > 0 ? 2 : (m_targetVelocity.X < 0 ? -2 : 0),
m_targetVelocity.Y > 0 ? 2 : (m_targetVelocity.Y < 0 ? -2 : 0),
0);
float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y) +
MinimumGroundFlightOffset;
//We cheat a bit and do a bit lower than normal
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude ||
(tempPos.Z - CAPSULE_LENGTH) <
_parent_scene.GetTerrainHeightAtXY(tempPos.X + forwardVel.X, tempPos.Y + forwardVel.Y)
+ MinimumGroundFlightOffset)
if (m_targetVelocity.Z < 0)
vec.Z += (target_altitude - tempPos.Z) * PID_D * 0.5f; //Don't apply so much
else if ((tempPos.Z - CAPSULE_LENGTH) + 5 < target_altitude)
vec.Z += (target_altitude - tempPos.Z) * PID_D * 3.05f;
else
vec.Z += (target_altitude - tempPos.Z) * PID_D * 1.75f;
}
else
{
//Straight up and down, only apply when they are very close to the ground
float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y);
if ((tempPos.Z - CAPSULE_LENGTH + (MinimumGroundFlightOffset / 1.5)) <
target_altitude + MinimumGroundFlightOffset)
{
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude + 1)
{
vec.Z += ((target_altitude + 4) - (tempPos.Z - CAPSULE_LENGTH)) * PID_D;
}
else
vec.Z += ((target_altitude + MinimumGroundFlightOffset) - (tempPos.Z - CAPSULE_LENGTH)) *
PID_D * 0.5f;
}
}
}
#endregion
#region Force application
#region Force push application
bool noDisable = false;
if (_target_force != Vector3.Zero)
{
_target_vel_force = _target_force/2;
_target_force = Vector3.Zero;
noDisable = true;
}
if (_target_vel_force.X != 0)
vec.X += (_target_vel_force.X)*PID_D*2;
if (_target_vel_force.Y != 0)
vec.Y += (_target_vel_force.Y)*PID_D*2;
if (_target_vel_force.Z != 0)
vec.Z += (_target_vel_force.Z)*PID_D;
#endregion
if (!flying && !IsTruelyColliding)
{
//Falling, and haven't yet hit the ground
vec.X += ((m_targetVelocity.X + m_forceAppliedBeforeFalling.X) * movementmult - vel.X) * PID_D * 2;
vec.Y += ((m_targetVelocity.Y + m_forceAppliedBeforeFalling.Y) * movementmult - vel.Y) * PID_D * 2;
vec.Z += ((m_targetVelocity.Z + m_forceAppliedBeforeFalling.Z) * movementmult - vel.Z) * PID_D;
}
else
{
m_forceAppliedBeforeFalling = !flying ? Vector3.Zero : m_targetVelocity;
vec.X += (m_targetVelocity.X * movementmult - vel.X) * PID_D * 2;
vec.Y += (m_targetVelocity.Y * movementmult - vel.Y) * PID_D * 2;
vec.Z += (m_targetVelocity.Z * movementmult - vel.Z) * PID_D;
}
Vector3 combinedForceVelocity = m_targetVelocity + _target_vel_force;
if ((combinedForceVelocity == Vector3.Zero ||
(Math.Abs(combinedForceVelocity.X) < 0.01f &&
Math.Abs(combinedForceVelocity.Y) < 0.01f &&
Math.Abs(combinedForceVelocity.Z) < 0.01f)) &&
Math.Abs(vel.X) < 0.1 && Math.Abs(vel.Y) < 0.1 &&
Math.Abs(vel.Z) < 0.1 && !(_appliedFallingForce > 0) && !noDisable)
{
//Body isn't moving, disable it
m_targetVelocity = Vector3.Zero;
_target_vel_force = Vector3.Zero;
d.BodySetLinearVel(Body, 0, 0, 0);
d.BodyDisable(Body);
}
else
{
m_targetVelocity *= _parent_scene.AvDecayTime;
_target_vel_force *= _parent_scene.AvDecayTime;
d.BodyEnable(Body);
d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
}
#endregion
}
public AuroraODEPrim(String primName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, float Density)
{
m_vehicle = new AuroraODEDynamics();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
ode = dode;
if (!pos.IsFinite())
{
pos = new Vector3((parent_scene.Region.RegionSizeX * 0.5f), (parent_scene.Region.RegionSizeY * 0.5f),
parent_scene.GetTerrainHeightAtXY((parent_scene.Region.RegionSizeX * 0.5f), (parent_scene.Region.RegionSizeY * 0.5f)));
m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
}
_position = pos;
fakepos = false;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
// correct for changed timestep
PID_D /= (parent_scene.ODE_STEPSIZE * 50f); // original ode fps of 50
PID_G /= (parent_scene.ODE_STEPSIZE * 50f);
m_density = Density / 100;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
prev_geom = IntPtr.Zero;
if (!size.IsFinite())
{
size = new Vector3(0.5f, 0.5f, 0.5f);
m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
}
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
if (!QuaternionIsFinite(rotation))
{
rotation = Quaternion.Identity;
m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
}
_orientation = rotation;
fakeori = false;
_mesh = mesh;
_pbs = pbs;
_parent_scene = parent_scene;
m_targetSpace = (IntPtr)0;
if (pos.Z < 0)
m_isphysical = false;
else
{
m_isphysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
if (m_isphysical)
m_targetSpace = _parent_scene.space;
}
m_primName = primName;
m_forceacc = Vector3.Zero;
m_angularforceacc = Vector3.Zero;
m_UpdateTimecntr = 0;
m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
m_UpdateTimecntr = .1f;
AddChange(changes.Add, null);
}
public override void AddAngularForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
AddChange(changes.AddAngForce, (object) force);
}
else
{
m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
}
}
/// <summary>
/// Adds the force supplied to the Target Velocity
/// The PID controller takes this target velocity and tries to make it a reality
/// </summary>
/// <param name="force"></param>
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
if (pushforce)
{
m_pidControllerActive = false;
force *= 100f;
doForce(force);
// If uncommented, things get pushed off world
//
// m_log.Debug("Push!");
// _target_velocity.X += force.X;
// _target_velocity.Y += force.Y;
// _target_velocity.Z += force.Z;
}
else
{
m_pidControllerActive = true;
_target_velocity.X += force.X;
_target_velocity.Y += force.Y;
_target_velocity.Z += force.Z;
}
}
else
{
m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
}
//m_lastUpdateSent = false;
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move(float timeStep, List<OdeCharacter> defects)
{
// 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 == IntPtr.Zero)
return;
if (m_pidControllerActive == false)
{
_zeroPosition = d.BodyGetPosition(Body);
}
//PidStatus = true;
d.Vector3 localpos = d.BodyGetPosition(Body);
Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
if (!localPos.IsFinite())
{
m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
_parent_scene.geom_name_map.Remove(Shell);
Shell = IntPtr.Zero;
}
return;
}
Vector3 vec = Vector3.Zero;
d.Vector3 vel = d.BodyGetLinearVel(Body);
float movementdivisor = 1f;
if (!m_alwaysRun)
{
movementdivisor = walkDivisor;
}
else
{
movementdivisor = runDivisor;
}
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
{
// keep track of where we stopped. No more slippin' & slidin'
if (!_zeroFlag)
{
_zeroFlag = true;
_zeroPosition = d.BodyGetPosition(Body);
}
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
d.Vector3 pos = d.BodyGetPosition(Body);
vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
if (flying)
{
vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
}
}
//PidStatus = true;
}
else
{
m_pidControllerActive = true;
_zeroFlag = false;
if (m_iscolliding && !flying)
{
// We're standing on something
vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
}
else if (m_iscolliding && flying)
{
// We're flying and colliding with something
vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16);
vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16);
}
else if (!m_iscolliding && flying)
{
// we're in mid air suspended
vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6);
vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6);
}
if (m_iscolliding && !flying && _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.
d.Vector3 pos = d.BodyGetPosition(Body);
vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
if (_target_velocity.X > 0)
{
vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
}
if (_target_velocity.Y > 0)
{
vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
}
}
else if (!m_iscolliding && !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 (_target_velocity.X > 0)
{
vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
}
if (_target_velocity.Y > 0)
{
vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
}
}
if (flying)
{
vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
}
}
if (flying)
{
vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
//Added for auto fly height. Kitto Flora
//d.Vector3 pos = d.BodyGetPosition(Body);
float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
if (_position.Z < target_altitude)
{
vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
}
// end add Kitto Flora
}
if (vec.IsFinite())
{
doForce(vec);
if (!_zeroFlag)
{
AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
}
}
else
{
m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
_parent_scene.geom_name_map.Remove(Shell);
Shell = IntPtr.Zero;
}
}
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move (float timeStep, ref List<AuroraODECharacter> defects)
{
// 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 == IntPtr.Zero)
return;
if (!m_shouldBePhysical)
return;
// replace amotor
d.Quaternion dtmp;
dtmp.W = 1;
dtmp.X = 0;
dtmp.Y = 0;
dtmp.Z = 0;
d.BodySetQuaternion (Body, ref dtmp);
d.BodySetAngularVel(Body, 0, 0, 0);
Vector3 vec = Vector3.Zero;
d.Vector3 vel = d.BodyGetLinearVel(Body);
#region Flight Ceiling
// rex, added height check
d.Vector3 tempPos = d.BodyGetPosition (Body);
if (m_pidControllerActive == false)
{
_zeroPosition = d.BodyGetPosition (Body);
}
if (_parent_scene.m_useFlightCeilingHeight && tempPos.Z > _parent_scene.m_flightCeilingHeight)
{
tempPos.Z = _parent_scene.m_flightCeilingHeight;
d.BodySetPosition (Body, tempPos.X, tempPos.Y, tempPos.Z);
d.Vector3 tempVel = d.BodyGetLinearVel (Body);
if (tempVel.Z > 0.0f)
{
tempVel.Z = 0.0f;
d.BodySetLinearVel (Body, tempVel.X, tempVel.Y, tempVel.Z);
}
if (_target_velocity.Z > 0.0f)
_target_velocity.Z = 0.0f;
}
// endrex
#endregion
#region NonFinite Pos
Vector3 localPos = new Vector3 ((float)tempPos.X, (float)tempPos.Y, (float)tempPos.Z);
if (!localPos.IsFinite ())
{
m_log.Warn ("[PHYSICS]: Avatar Position is non-finite!");
defects.Add (this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
/*
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
*/
//kill the Geometry
_parent_scene.waitForSpaceUnlock (_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy (Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy (Shell);
_parent_scene.geom_name_map.Remove (Shell);
Shell = IntPtr.Zero;
}
return;
}
#endregion
#region Check for out of region
if (Position.X < 0.25f || Position.Y < 0.25f ||
Position.X > _parent_scene.Region.RegionSizeX - .25f ||
Position.Y > _parent_scene.Region.RegionSizeY - .25f)
{
if (!CheckForRegionCrossing())
{
Vector3 newPos = Position;
newPos.X = Util.Clip(Position.X, 0.75f, _parent_scene.Region.RegionSizeX - 0.75f);
newPos.Y = Util.Clip(Position.Y, 0.75f, _parent_scene.Region.RegionSizeY - 0.75f);
Position = newPos;
d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
}
}
#endregion
#region Movement Multiplier
float movementmult = 1f;
if (!m_alwaysRun)
movementmult /= _parent_scene.avMovementDivisorWalk;
else
movementmult /= _parent_scene.avMovementDivisorRun;
movementmult *= 10;
movementmult *= SpeedModifier;
if (flying)
movementmult *= _parent_scene.m_AvFlySpeed;
#endregion
#region Check for underground
if (!(Position.X < 0.25f || Position.Y < 0.25f ||
Position.X > _parent_scene.Region.RegionSizeX - .25f ||
Position.Y > _parent_scene.Region.RegionSizeY - .25f))
{
float groundHeight = _parent_scene.GetTerrainHeightAtXY (
tempPos.X + (tempPos.X == 0 ? tempPos.X : timeStep * 0.75f * vel.X),
tempPos.Y + (tempPos.Y == 0 ? tempPos.Y : timeStep * 0.75f * vel.Y));
if ((tempPos.Z - AvatarHalfsize) < groundHeight)
{
if (!flying)
{
//if (_target_velocity.Z < 0)
_target_velocity.Z = 0;
vec.Z = -vel.Z * PID_D * 2f + ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P * 100.0f);
}
else
vec.Z = ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P);
}
if (tempPos.Z - AvatarHalfsize - groundHeight < 0.12f)
{
m_iscolliding = true;
m_iscollidingGround = true;
flying = false; // ground the avatar
ContactPoint point = new ContactPoint ();
point.PenetrationDepth = vel.Z;
point.Position = Position;
point.SurfaceNormal = Vector3.Zero;
//0 is the ground localID
this.AddCollisionEvent (0, point);
}
else
m_iscollidingGround = false;
}
else
m_iscollidingGround = true;//Assume that they are crossing, and keep them from falling down
#endregion
#region Movement
#region Jump code
if (IsJumping && (IsColliding && m_preJumpCounter > _parent_scene.m_preJumpTime + 10) || m_preJumpCounter > 50)
{
m_isJumping = false;
m_preJumpCounter = 0;
}
if (IsJumping)
m_preJumpCounter++;
if (m_ispreJumping && m_preJumpCounter == _parent_scene.m_preJumpTime)
{
m_ispreJumping = false;
_target_velocity += m_preJumpForce * _parent_scene.m_preJumpForceMultiplier;
m_preJumpCounter = 0;
m_isJumping = true;
}
if (m_ispreJumping)
{
m_preJumpCounter++;
TriggerMovementUpdate();
return;
}
#endregion
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity == Vector3.Zero &&
Math.Abs (vel.X) < 0.05 && Math.Abs (vel.Y) < 0.05 && Math.Abs (vel.Z) < 0.05 && (this.m_iscollidingGround || this.m_iscollidingObj || this.flying))
//This is so that if we get moved by something else, it will update us in the client
{
m_isJumping = false;
// keep track of where we stopped. No more slippin' & slidin'
if (!_zeroFlag)
{
_zeroFlag = true;
_zeroPosition = tempPos;
}
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
// if target vel is zero why was it here ?
vec.X = -vel.X * PID_D + (_zeroPosition.X - tempPos.X) * PID_P;
vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - tempPos.Y) * PID_P;
}
}
else
{
m_pidControllerActive = true;
_zeroFlag = false;
if (m_iscolliding)
{
if (!flying)
{
if (_target_velocity.Z != 0.0f)
vec.Z = (_target_velocity.Z * movementmult - vel.Z) * PID_D;// + (_zeroPosition.Z - tempPos.Z) * PID_P)) _zeropos maybe bad here
// We're standing or walking on something
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 2;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 2;
}
else
{
// We're flying and colliding with something
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.5f;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.5f;
}
}
else
{
if (flying)
{
// we're flying
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.75f;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.75f;
}
else
{
// we're not colliding and we're not flying so that means we're falling!
// m_iscolliding includes collisions with the ground.
vec.X = (_target_velocity.X - vel.X) * PID_D * 0.85f;
vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.85f;
}
}
if (flying)
{
#region Av gravity
if (_parent_scene.AllowAvGravity &&
tempPos.Z > _parent_scene.AvGravityHeight) //Should be stop avies from flying upwards
{
//Decay going up
if (_target_velocity.Z > 0)
{
//How much should we force them down?
float Multiplier = (_parent_scene.AllowAvsToEscapeGravity ? .03f : .1f);
//How much should we force them down?
float fudgeHeight = (_parent_scene.AllowAvsToEscapeGravity ? 80 : 30);
//We add the 30 so that gravity is resonably strong once they pass the min height
Multiplier *= tempPos.Z + fudgeHeight - _parent_scene.AvGravityHeight;
//Limit these so that things don't go wrong
if (Multiplier < 1)
Multiplier = 1;
float maxpower = (_parent_scene.AllowAvsToEscapeGravity ? 1.5f : 3f);
if (Multiplier > maxpower)
Multiplier = maxpower;
_target_velocity.Z /= Multiplier;
vel.Z /= Multiplier;
}
}
#endregion
vec.Z = (_target_velocity.Z * movementmult - vel.Z) * PID_D * 0.5f;
if (_parent_scene.AllowAvGravity && tempPos.Z > _parent_scene.AvGravityHeight)
//Add extra gravity
vec.Z += ((10 * _parent_scene.gravityz) * Mass);
}
}
if (flying)
{
#region Auto Fly Height
//Added for auto fly height. Kitto Flora
//Changed to only check if the avatar is flying around,
// Revolution: If the avatar is going down, they are trying to land (probably), so don't push them up to make it harder
// Only if they are moving around sideways do we need to push them up
if (_target_velocity.X != 0 || _target_velocity.Y != 0)
{
Vector3 forwardVel = new Vector3 (_target_velocity.X > 0 ? 2 : (_target_velocity.X < 0 ? -2 : 0),
_target_velocity.Y > 0 ? 2 : (_target_velocity.Y < 0 ? -2 : 0),
0);
float target_altitude = _parent_scene.GetTerrainHeightAtXY (tempPos.X, tempPos.Y) + MinimumGroundFlightOffset;
//We cheat a bit and do a bit lower than normal
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude ||
(tempPos.Z - CAPSULE_LENGTH) < _parent_scene.GetTerrainHeightAtXY (tempPos.X + forwardVel.X, tempPos.Y + forwardVel.Y)
+ MinimumGroundFlightOffset)
vec.Z += (target_altitude - tempPos.Z) * PID_P * 0.5f;
}
else
{
//Straight up and down, only apply when they are very close to the ground
float target_altitude = _parent_scene.GetTerrainHeightAtXY (tempPos.X, tempPos.Y);
if ((tempPos.Z - CAPSULE_LENGTH + (MinimumGroundFlightOffset / 1.5)) < target_altitude + MinimumGroundFlightOffset)
{
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude + 1)
{
vec.Z += ((target_altitude + 4) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P;
}
else
vec.Z += ((target_altitude + MinimumGroundFlightOffset) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P * 0.5f;
}
}
#endregion
}
#region Gravity
if (!flying && _parent_scene.AllowAvGravity)
{
if (!_parent_scene.UsePointGravity)
{
//Add normal gravity
vec.X += _parent_scene.gravityx * m_mass;
vec.Y += _parent_scene.gravityy * m_mass;
vec.Z += _parent_scene.gravityz * m_mass;
}
else
{
Vector3 cog = _parent_scene.PointOfGravity;
if (cog.X != 0)
vec.X += (cog.X - tempPos.X) * m_mass;
if (cog.Y != 0)
vec.Y += (cog.Y - tempPos.Y) * m_mass;
if (cog.Z != 0)
vec.Z += (cog.Z - tempPos.Z) * m_mass;
}
}
#endregion
#region Under water physics
if (_parent_scene.AllowUnderwaterPhysics && (float)tempPos.X < _parent_scene.Region.RegionSizeX &&
(float)tempPos.Y < _parent_scene.Region.RegionSizeY)
{
//Position plus height to av's shoulder (aprox) is just above water
if ((tempPos.Z + (CAPSULE_LENGTH / 3) - .25f) < _parent_scene.GetWaterLevel ((float)tempPos.X, (float)tempPos.Y))
{
if (StartingUnderWater)
ShouldBeWalking = Flying == false;
StartingUnderWater = false;
WasUnderWater = true;
Flying = true;
lastUnderwaterPush = 0;
if (ShouldBeWalking)
{
lastUnderwaterPush += (float)(_parent_scene.GetWaterLevel ((float)tempPos.X, (float)tempPos.Y) - tempPos.Z) * 33 + 3;
vec.Z += lastUnderwaterPush;
}
else
{
lastUnderwaterPush += 3500;
lastUnderwaterPush += (float)(_parent_scene.GetWaterLevel ((float)tempPos.X, (float)tempPos.Y) - tempPos.Z) * 8;
vec.Z += lastUnderwaterPush;
}
}
else
{
StartingUnderWater = true;
if (WasUnderWater)
{
WasUnderWater = false;
Flying = true;
}
}
}
#endregion
#endregion
#region Apply the force
if (vec.IsFinite ())
{
if (vec.X < 100000000 && vec.Y < 10000000 && vec.Z < 10000000) //Checks for crazy, going to NaN us values
{
// round small values to zero. those possible are just errors
if (Math.Abs (vec.X) < 0.001)
vec.X = 0;
if (Math.Abs (vec.Y) < 0.001)
vec.Y = 0;
if (Math.Abs (vec.Z) < 0.001)
vec.Z = 0;
if (vec == Vector3.Zero) //if we arn't moving, STOP
d.BodySetLinearVel(Body, vec.X, vec.Y, vec.Z);
else
doForce (vec);
//When falling, we keep going faster and faster, and eventually, the client blue screens (blue is all you see).
// The speed that does this is slightly higher than -30, so we cap it here so we never do that during falling.
if (vel.Z < -30)
{
vel.Z = -30;
d.BodySetLinearVel (Body, vel.X, vel.Y, vel.Z);
}
//Decay out the target velocity
_target_velocity *= _parent_scene.m_avDecayTime;
if (!_zeroFlag && _target_velocity.ApproxEquals (Vector3.Zero, _parent_scene.m_avStopDecaying))
_target_velocity = Vector3.Zero;
}
else
{
//This is a safe guard from going NaN, but it isn't very smooth... which is ok
d.BodySetForce (Body, 0, 0, 0);
d.BodySetLinearVel (Body, 0, 0, 0);
}
}
else
{
m_log.Warn ("[PHYSICS]: Got a NaN force vector in Move()");
m_log.Warn ("[PHYSICS]: Avatar Position is non-finite!");
defects.Add (this);
//kill the Geometry
_parent_scene.waitForSpaceUnlock (_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy (Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy (Shell);
_parent_scene.geom_name_map.Remove (Shell);
Shell = IntPtr.Zero;
}
}
#endregion
}
public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
{
m_uuid = UUID.Random();
if (pos.IsFinite())
{
if (pos.Z > 9999999f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
if (pos.Z < -90000f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
_position = pos;
m_taintPosition.X = pos.X;
m_taintPosition.Y = pos.Y;
m_taintPosition.Z = pos.Z;
}
else
{
_position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
m_taintPosition.X = _position.X;
m_taintPosition.Y = _position.Y;
m_taintPosition.Z = _position.Z;
m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
_parent_scene = parent_scene;
PID_D = pid_d;
PID_P = pid_p;
CAPSULE_RADIUS = capsule_radius;
m_tensor = tensor;
m_density = density;
heightFudgeFactor = height_fudge_factor;
walkDivisor = walk_divisor;
runDivisor = rundivisor;
// m_StandUpRotation =
// new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
// 0.5f);
for (int i = 0; i < 11; i++)
{
m_colliderarr[i] = false;
}
CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
_parent_scene.AddPhysicsActorTaint(this);
m_name = avName;
}
public AuroraODECharacter(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation, Vector3 size)
{
m_uuid = UUID.Random();
m_taintRotation = rotation;
// _orientation = rotation;
// _lastorientation = rotation;
if (pos.IsFinite())
{
if (pos.Z > 9999999f || pos.Z <-90f)
{
// pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
pos.Z = parent_scene.GetTerrainHeightAtXY(parent_scene.Region.RegionSizeX * 0.5f, parent_scene.Region.RegionSizeY * 0.5f) + 5.0f;
}
_position = pos;
m_taintPosition.X = pos.X;
m_taintPosition.Y = pos.Y;
m_taintPosition.Z = pos.Z;
}
else
{
_position.X = (float)parent_scene.Region.RegionSizeX * 0.5f;
_position.Y = (float)parent_scene.Region.RegionSizeY * 0.5f;
_position.Z = parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + 10f;
m_taintPosition.X = _position.X;
m_taintPosition.Y = _position.Y;
m_taintPosition.Z = _position.Z;
m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
_parent_scene = parent_scene;
CAPSULE_RADIUS = parent_scene.avCapRadius;
// m_StandUpRotation =
// new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
// 0.5f);
CAPSULE_LENGTH = (size.Z * 1.1f) - CAPSULE_RADIUS * 2.0f;
if ((m_collisionFlags & CollisionCategories.Land) == 0)
AvatarHalfsize = CAPSULE_LENGTH * 0.5f + CAPSULE_RADIUS;
else
AvatarHalfsize = CAPSULE_LENGTH * 0.5f + CAPSULE_RADIUS - 0.3f;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
_parent_scene.AddPhysicsActorTaint(this);
m_UpdateTimecntr = 0;
m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
m_UpdateTimecntr = .1f;
m_name = avName;
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move(List<OdeCharacter> defects)
{
if (Body == IntPtr.Zero)
return;
d.Vector3 dtmp = d.BodyGetPosition(Body);
Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
// the Amotor still lets avatar rotation to drift during colisions
// so force it back to identity
d.Quaternion qtmp;
qtmp.W = m_orientation2D.W;
qtmp.X = m_orientation2D.X;
qtmp.Y = m_orientation2D.Y;
qtmp.Z = m_orientation2D.Z;
d.BodySetQuaternion(Body, ref qtmp);
if (m_pidControllerActive == false)
{
_zeroPosition = localpos;
}
if (!localpos.IsFinite())
{
m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
AvatarGeomAndBodyDestroy();
return;
}
// check outbounds forcing to be in world
bool fixbody = false;
if (localpos.X < 0.0f)
{
fixbody = true;
localpos.X = 0.1f;
}
else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
{
fixbody = true;
localpos.X = _parent_scene.WorldExtents.X - 0.1f;
}
if (localpos.Y < 0.0f)
{
fixbody = true;
localpos.Y = 0.1f;
}
else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
{
fixbody = true;
localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
}
if (fixbody)
{
m_freemove = false;
d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
}
float breakfactor;
Vector3 vec = Vector3.Zero;
dtmp = d.BodyGetLinearVel(Body);
Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
float velLengthSquared = vel.LengthSquared();
Vector3 ctz = _target_velocity;
float movementdivisor = 1f;
//Ubit change divisions into multiplications below
if (!m_alwaysRun)
movementdivisor = 1 / walkDivisor;
else
movementdivisor = 1 / runDivisor;
ctz.X *= movementdivisor;
ctz.Y *= movementdivisor;
//******************************************
// colide with land
d.AABB aabb;
// d.GeomGetAABB(feetbox, out aabb);
d.GeomGetAABB(capsule, out aabb);
float chrminZ = aabb.MinZ; // move up a bit
Vector3 posch = localpos;
float ftmp;
if (flying)
{
ftmp = timeStep;
posch.X += vel.X * ftmp;
posch.Y += vel.Y * ftmp;
}
float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
if (chrminZ < terrainheight)
{
if (ctz.Z < 0)
ctz.Z = 0;
Vector3 n = _parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y);
float depth = terrainheight - chrminZ;
vec.Z = depth * PID_P * 50;
if (!flying)
vec.Z += -vel.Z * PID_D;
if (depth < 0.2f)
{
m_colliderGroundfilter++;
if (m_colliderGroundfilter > 2)
{
m_iscolliding = true;
m_colliderfilter = 2;
if (m_colliderGroundfilter > 10)
{
m_colliderGroundfilter = 10;
m_freemove = false;
}
m_collideNormal.X = n.X;
m_collideNormal.Y = n.Y;
m_collideNormal.Z = n.Z;
m_iscollidingGround = true;
ContactPoint contact = new ContactPoint();
contact.PenetrationDepth = depth;
contact.Position.X = localpos.X;
contact.Position.Y = localpos.Y;
contact.Position.Z = terrainheight;
contact.SurfaceNormal.X = -n.X;
contact.SurfaceNormal.Y = -n.Y;
contact.SurfaceNormal.Z = -n.Z;
contact.RelativeSpeed = -vel.Z;
contact.CharacterFeet = true;
AddCollisionEvent(0, contact);
// vec.Z *= 0.5f;
}
}
else
{
m_colliderGroundfilter -= 5;
if (m_colliderGroundfilter <= 0)
{
m_colliderGroundfilter = 0;
m_iscollidingGround = false;
}
}
}
else
{
m_colliderGroundfilter -= 5;
if (m_colliderGroundfilter <= 0)
{
m_colliderGroundfilter = 0;
m_iscollidingGround = false;
}
}
bool hoverPIDActive = false;
if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
{
hoverPIDActive = true;
switch (m_PIDHoverType)
{
case PIDHoverType.Ground:
m_targetHoverHeight = terrainheight + m_PIDHoverHeight;
break;
case PIDHoverType.GroundAndWater:
float waterHeight = _parent_scene.GetWaterLevel();
if (terrainheight > waterHeight)
m_targetHoverHeight = terrainheight + m_PIDHoverHeight;
else
m_targetHoverHeight = waterHeight + m_PIDHoverHeight;
break;
} // end switch (m_PIDHoverType)
// don't go underground
if (m_targetHoverHeight > terrainheight + 0.5f * (aabb.MaxZ - aabb.MinZ))
{
float fz = (m_targetHoverHeight - localpos.Z);
// if error is zero, use position control; otherwise, velocity control
if (Math.Abs(fz) < 0.01f)
{
ctz.Z = 0;
}
else
{
_zeroFlag = false;
fz /= m_PIDHoverTau;
float tmp = Math.Abs(fz);
if (tmp > 50)
fz = 50 * Math.Sign(fz);
else if (tmp < 0.1)
fz = 0.1f * Math.Sign(fz);
ctz.Z = fz;
}
}
}
//******************************************
if (!m_iscolliding)
m_collideNormal.Z = 0;
bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f);
if (!tviszero)
{
m_freemove = false;
// movement relative to surface if moving on it
// dont disturbe vertical movement, ie jumps
if (m_iscolliding && !flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f)
{
float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y;
ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X);
ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y);
ctz.Z -= p;
if (ctz.Z < 0)
ctz.Z *= 2;
}
}
if (!m_freemove)
{
// if velocity is zero, use position control; otherwise, velocity control
if (tviszero && m_iscolliding && !flying)
{
// keep track of where we stopped. No more slippin' & slidin'
if (!_zeroFlag)
{
_zeroFlag = true;
_zeroPosition = localpos;
}
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
vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5);
vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5);
if(vel.Z > 0)
vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
else
vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f;
/*
if (flying)
{
vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
}
*/
}
//PidStatus = true;
}
else
{
m_pidControllerActive = true;
_zeroFlag = false;
if (m_iscolliding)
{
if (!flying)
{
// we are on a surface
if (ctz.Z > 0f)
{
// moving up or JUMPING
vec.Z += (ctz.Z - vel.Z) * PID_D * 2f;
vec.X += (ctz.X - vel.X) * (PID_D);
vec.Y += (ctz.Y - vel.Y) * (PID_D);
}
else
{
// we are moving down on a surface
if (ctz.Z == 0)
{
if (vel.Z > 0)
vec.Z -= vel.Z * PID_D * 2f;
vec.X += (ctz.X - vel.X) * (PID_D);
vec.Y += (ctz.Y - vel.Y) * (PID_D);
}
// intencionally going down
else
{
if (ctz.Z < vel.Z)
vec.Z += (ctz.Z - vel.Z) * PID_D;
else
{
}
if (Math.Abs(ctz.X) > Math.Abs(vel.X))
vec.X += (ctz.X - vel.X) * (PID_D);
if (Math.Abs(ctz.Y) > Math.Abs(vel.Y))
vec.Y += (ctz.Y - vel.Y) * (PID_D);
}
}
// We're standing on something
}
else
{
// We're flying and colliding with something
vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f);
vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f);
vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f);
}
}
else // ie not colliding
{
if (flying || hoverPIDActive) //(!m_iscolliding && flying)
{
// we're in mid air suspended
vec.X += (ctz.X - vel.X) * (PID_D);
vec.Y += (ctz.Y - vel.Y) * (PID_D);
vec.Z += (ctz.Z - vel.Z) * (PID_D);
}
else
{
// 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);
vec.X += (ctz.X - vel.X) * PID_D * 0.833f;
vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f;
// hack for breaking on fall
if (ctz.Z == -9999f)
vec.Z += -vel.Z * PID_D - _parent_scene.gravityz * m_mass;
}
}
}
if (velLengthSquared > 2500.0f) // 50m/s apply breaks
{
breakfactor = 0.16f * m_mass;
vec.X -= breakfactor * vel.X;
vec.Y -= breakfactor * vel.Y;
vec.Z -= breakfactor * vel.Z;
}
}
else
{
breakfactor = m_mass;
vec.X -= breakfactor * vel.X;
vec.Y -= breakfactor * vel.Y;
if (flying)
vec.Z -= 0.5f * breakfactor * vel.Z;
else
vec.Z -= .16f* m_mass * vel.Z;
}
if (flying || hoverPIDActive)
{
vec.Z -= _parent_scene.gravityz * m_mass;
if(!hoverPIDActive)
{
//Added for auto fly height. Kitto Flora
float target_altitude = terrainheight + MinimumGroundFlightOffset;
if (localpos.Z < target_altitude)
{
vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
}
// end add Kitto Flora
}
}
if (vec.IsFinite())
{
if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
}
else
{
m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
AvatarGeomAndBodyDestroy();
return;
}
// update our local ideia of position velocity and aceleration
// _position = localpos;
_position = localpos;
if (_zeroFlag)
{
_velocity = Vector3.Zero;
_acceleration = Vector3.Zero;
m_rotationalVelocity = Vector3.Zero;
}
else
{
Vector3 a =_velocity; // previus velocity
SetSmooth(ref _velocity, ref vel, 2);
a = (_velocity - a) * invtimeStep;
SetSmooth(ref _acceleration, ref a, 2);
dtmp = d.BodyGetAngularVel(Body);
m_rotationalVelocity.X = 0f;
m_rotationalVelocity.Y = 0f;
m_rotationalVelocity.Z = dtmp.Z;
Math.Round(m_rotationalVelocity.Z,3);
}
}
public override void SetMomentum(Vector3 momentum)
{
if (momentum.IsFinite())
AddChange(changes.Momentum, momentum);
}
public override void AddAngularForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
m_angularforcelist.Add(force);
m_taintaddangularforce = true;
}
else
{
m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
}
}
public WhiteCoreODECharacter(String avName, WhiteCoreODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation,
Vector3 size)
{
m_uuid = UUID.Random();
_parent_scene = parent_scene;
m_taintRotation = rotation;
if (pos.IsFinite())
{
if (pos.Z > 9999999f || pos.Z < -90f)
{
pos.Z =
_parent_scene.GetTerrainHeightAtXY(_parent_scene.Region.RegionSizeX*0.5f,
_parent_scene.Region.RegionSizeY*0.5f) + 5.0f;
}
_position = pos;
}
else
{
_position.X = _parent_scene.Region.RegionSizeX*0.5f;
_position.Y = _parent_scene.Region.RegionSizeY*0.5f;
_position.Z = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + 10f;
MainConsole.Instance.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
m_isPhysical = false; // current status: no ODE information exists
Size = size;
Name = avName;
}
public override void LockAngularMotion(Vector3 axis)
{
// reverse the zero/non zero values for ODE.
if (axis.IsFinite())
{
axis.X = (axis.X > 0) ? 1f : 0f;
axis.Y = (axis.Y > 0) ? 1f : 0f;
axis.Z = (axis.Z > 0) ? 1f : 0f;
m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
m_taintAngularLock = axis;
}
else
{
m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
}
}
/// <summary>
/// Adds the force supplied to the Target Velocity
/// The PID controller takes this target velocity and tries to make it a reality
/// </summary>
/// <param name="force"></param>
/// <param name="pushforce"></param>
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
if (pushforce)
{
_parent_scene.AddSimulationChange(() =>
{
if (Body != IntPtr.Zero)
_target_force = force;
});
}
else
{
m_targetVelocity.X += force.X;
m_targetVelocity.Y += force.Y;
m_targetVelocity.Z += force.Z;
}
}
else
{
MainConsole.Instance.Warn("[PHYSICS]: Got a NaN force applied to a Character");
}
//m_lastUpdateSent = false;
}
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
AddChange(changes.Force, (object) force);
}
else
{
m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
}
//m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
}
public override void LockAngularMotion(Vector3 axis)
{
// reverse the zero/non zero values for ODE.
if (axis.IsFinite())
{
axis.X = (axis.X > 0) ? 1f : 0f;
axis.Y = (axis.Y > 0) ? 1f : 0f;
axis.Z = (axis.Z > 0) ? 1f : 0f;
MainConsole.Instance.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
_parent_scene.AddSimulationChange(() => changeAngularLock(axis));
}
else
{
MainConsole.Instance.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
}
}
public override void LockAngularMotion(Vector3 axis)
{
// reverse the zero/non zero values for ODE.
if (axis.IsFinite())
{
axis.X = (axis.X > 0) ? 1f : 0f;
axis.Y = (axis.Y > 0) ? 1f : 0f;
axis.Z = (axis.Z > 0) ? 1f : 0f;
m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
AddChange(changes.AngLock, (object)axis);
}
else
{
m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
}
}
public override void AddAngularForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
_parent_scene.AddSimulationChange(() => changeAddAngularForce(force));
else
MainConsole.Instance.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
}
/// <summary>
/// This is the event handler for client movement. If a client is moving, this event is triggering.
/// </summary>
public virtual void HandleAgentUpdate(IClientAPI remoteClient, AgentUpdateArgs agentData)
{
m_perfMonMS = Util.EnvironmentTickCount();
++m_movementUpdateCount;
if (m_movementUpdateCount < 1)
m_movementUpdateCount = 1;
#region Sanity Checking
// This is irritating. Really.
if (!AbsolutePosition.IsFinite())
{
OutOfBoundsCall(Vector3.Zero);
return;
}
#endregion Sanity Checking
#region Inputs
if (Frozen)
return; //Do nothing, just end
AgentManager.ControlFlags flags = (AgentManager.ControlFlags) agentData.ControlFlags;
Quaternion bodyRotation = agentData.BodyRotation;
//Check to see whether ray casting needs done
// We multiply by 10 so that we don't trigger it when the camera moves slightly (as its 2 meter change)
if (Util.GetFlatDistanceTo(agentData.CameraCenter, m_lastCameraCenter) > SIGNIFICANT_MOVEMENT*10)
{
m_lastCameraCenter = agentData.CameraCenter;
Scene.AuroraEventManager.FireGenericEventHandler("SignficantCameraMovement", this);
}
// Camera location in world. We'll need to raytrace
// from this location from time to time.
m_CameraCenter = agentData.CameraCenter;
// Use these three vectors to figure out what the agent is looking at
// Convert it to a Matrix and/or Quaternion
m_CameraAtAxis = agentData.CameraAtAxis;
m_CameraLeftAxis = agentData.CameraLeftAxis;
m_CameraUpAxis = agentData.CameraUpAxis;
// The Agent's Draw distance setting
DrawDistance = agentData.Far;
// Check if Client has camera in 'follow cam' or 'build' mode.
Vector3 camdif = (Vector3.One*m_bodyRot - Vector3.One*CameraRotation);
m_followCamAuto = ((m_CameraUpAxis.Z > 0.959f && m_CameraUpAxis.Z < 0.98f)
&& (Math.Abs(camdif.X) < 0.4f && Math.Abs(camdif.Y) < 0.4f));
m_mouseLook = (flags & AgentManager.ControlFlags.AGENT_CONTROL_MOUSELOOK) != 0;
m_leftButtonDown = (flags & AgentManager.ControlFlags.AGENT_CONTROL_LBUTTON_DOWN) != 0;
m_isAway = (flags & AgentManager.ControlFlags.AGENT_CONTROL_AWAY) != 0;
#endregion Inputs
if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_STAND_UP) != 0)
{
StandUp();
}
//MainConsole.Instance.DebugFormat("[FollowCam]: {0}", m_followCamAuto);
// Raycast from the avatar's head to the camera to see if there's anything blocking the view
if ((m_movementUpdateCount%NumMovementsBetweenRayCast) == 0 && m_scene.PhysicsScene.SupportsRayCast())
{
if (m_followCamAuto)
{
Vector3 posAdjusted = m_pos + HEAD_ADJUSTMENT;
m_scene.PhysicsScene.RaycastWorld(m_pos, Vector3.Normalize(m_CameraCenter - posAdjusted),
Vector3.Distance(m_CameraCenter, posAdjusted) + 0.3f,
RayCastCameraCallback);
}
}
if (!m_CameraCenter.IsFinite())
{
m_CameraCenter = new Vector3(128, 128, 128);
}
IScriptControllerModule m = RequestModuleInterface<IScriptControllerModule>();
if (m != null) //Tell any scripts about it
m.OnNewMovement(ref flags);
if (m_autopilotMoving)
CheckAtSitTarget();
// In the future, these values might need to go global.
// Here's where you get them.
if (!SitGround)
SitGround = (flags & AgentManager.ControlFlags.AGENT_CONTROL_SIT_ON_GROUND) != 0;
m_AgentControlFlags = flags;
m_headrotation = agentData.HeadRotation;
m_state = agentData.State;
PhysicsActor actor = PhysicsActor;
if (actor == null)
{
//This happens while sitting, don't spam it
//MainConsole.Instance.Debug("Null physical actor in AgentUpdate in " + m_scene.RegionInfo.RegionName);
return;
}
bool update_movementflag = false;
bool update_rotation = false;
if (AllowMovement && !SitGround && !Frozen)
{
if (FallenStandUp)
{
//Poke the animator a bit
Animator.UpdateMovementAnimations(false);
m_bodyRot = bodyRotation;
AddNewMovement(Vector3.Zero, bodyRotation);
return;
}
if (agentData.UseClientAgentPosition)
{
m_moveToPositionInProgress = (agentData.ClientAgentPosition - AbsolutePosition).Length() > 0.2f;
m_moveToPositionTarget = agentData.ClientAgentPosition;
}
int i = 0;
bool DCFlagKeyPressed = false;
Vector3 agent_control_v3 = Vector3.Zero;
Quaternion q = bodyRotation;
bool oldflying = PhysicsActor.Flying;
if (m_forceFly)
actor.Flying = true;
else if (m_flyDisabled)
actor.Flying = false;
else if (actor.Flying != ((flags & AgentManager.ControlFlags.AGENT_CONTROL_FLY) != 0))
actor.Flying = ((flags & AgentManager.ControlFlags.AGENT_CONTROL_FLY) != 0);
if (actor.Flying != oldflying)
update_movementflag = true;
if (q != m_bodyRot)
{
Quaternion delta = Quaternion.Inverse(m_bodyRot)*q;
m_bodyRot = q;
if (!(Math.Abs(delta.X) < 1e-5f && Math.Abs(delta.Y) < 1e-5f && Math.Abs(delta.Z) < 1e-5f))
update_rotation = true;
}
if (m_parentID == UUID.Zero)
{
bool bAllowUpdateMoveToPosition = false;
bool bResetMoveToPosition = false;
Vector3[] dirVectors;
// use camera up angle when in mouselook and not flying or when holding the left mouse button down and not flying
// this prevents 'jumping' in inappropriate situations.
if ((m_mouseLook && !m_physicsActor.Flying) || (m_leftButtonDown && !m_physicsActor.Flying))
dirVectors = GetWalkDirectionVectors();
else
dirVectors = Dir_Vectors;
// The fact that m_movementflag is a byte needs to be fixed
// it really should be a uint
const uint nudgehack = 250;
//Do these two like this to block out all others because it will slow it down
if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_POS) ==
AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_POS)
{
bResetMoveToPosition = true;
DCFlagKeyPressed = true;
agent_control_v3 += dirVectors[8];
}
else if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_NEG) ==
AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_NEG)
{
bResetMoveToPosition = true;
DCFlagKeyPressed = true;
agent_control_v3 += dirVectors[9];
}
else
{
foreach (Dir_ControlFlags DCF in DIR_CONTROL_FLAGS)
{
if (((uint) flags & (uint) DCF) != 0)
{
bResetMoveToPosition = true;
DCFlagKeyPressed = true;
agent_control_v3 += dirVectors[i];
//MainConsole.Instance.DebugFormat("[Motion]: {0}, {1}",i, dirVectors[i]);
if ((m_movementflag & (uint) DCF) == 0)
{
if (DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE ||
DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
{
// m_movementflag |= (byte)nudgehack;
m_movementflag |= nudgehack;
}
m_movementflag += (uint) DCF;
update_movementflag = true;
}
}
else
{
if ((m_movementflag & (uint) DCF) != 0 ||
((DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE ||
DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
&& ((m_movementflag & nudgehack) == nudgehack))
) // This or is for Nudge forward
{
m_movementflag -= ((uint) DCF);
update_movementflag = true;
/*
if ((DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
&& ((m_movementflag & (byte)nudgehack) == nudgehack))
{
MainConsole.Instance.Debug("Removed Hack flag");
}
*/
}
else
{
bAllowUpdateMoveToPosition = true;
}
}
i++;
}
}
//Paupaw:Do Proper PID for Autopilot here
if (bResetMoveToPosition)
{
m_moveToPositionTarget = Vector3.Zero;
m_moveToPositionInProgress = false;
update_movementflag = true;
bAllowUpdateMoveToPosition = false;
}
if (bAllowUpdateMoveToPosition && (m_moveToPositionInProgress && !m_autopilotMoving))
{
//Check the error term of the current position in relation to the target position
if (Util.GetFlatDistanceTo(AbsolutePosition, m_moveToPositionTarget) <= 0.5f)
{
// we are close enough to the target
m_moveToPositionTarget = Vector3.Zero;
m_moveToPositionInProgress = false;
update_movementflag = true;
}
else
{
try
{
// move avatar in 2D at one meter/second towards target, in avatar coordinate frame.
// This movement vector gets added to the velocity through AddNewMovement().
// Theoretically we might need a more complex PID approach here if other
// unknown forces are acting on the avatar and we need to adaptively respond
// to such forces, but the following simple approach seems to works fine.
Vector3 LocalVectorToTarget3D =
(m_moveToPositionTarget - AbsolutePosition)
// vector from cur. pos to target in global coords
// * Matrix4.CreateFromQuaternion(Quaternion.Inverse(bodyRotation)); // change to avatar coords
*Quaternion.Inverse(bodyRotation);
// mult by matix is faster but with creation, use *quarternion
// Ignore z component of vector
Vector3 LocalVectorToTarget2D;
LocalVectorToTarget2D.X = LocalVectorToTarget3D.X;
LocalVectorToTarget2D.Y = LocalVectorToTarget3D.Y;
LocalVectorToTarget2D.Z = 0f;
agent_control_v3 += LocalVectorToTarget2D;
// update avatar movement flags. the avatar coordinate system is as follows:
//
// +X (forward)
//
// ^
// |
// |
// |
// |
// (left) +Y <--------o--------> -Y
// avatar
// |
// |
// |
// |
// v
// -X
//
// based on the above avatar coordinate system, classify the movement into
// one of left/right/back/forward.
if (LocalVectorToTarget2D.Y > 0) //MoveLeft
{
m_movementflag += (uint) Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
//AgentControlFlags
AgentControlFlags |= (uint) Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
update_movementflag = true;
}
else if (LocalVectorToTarget2D.Y < 0) //MoveRight
{
m_movementflag += (uint) Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT;
AgentControlFlags |= (uint) Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT;
update_movementflag = true;
}
if (LocalVectorToTarget2D.X < 0) //MoveBack
{
m_movementflag += (uint) Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
AgentControlFlags |= (uint) Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
update_movementflag = true;
}
else if (LocalVectorToTarget2D.X > 0) //Move Forward
{
m_movementflag += (uint) Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
AgentControlFlags |= (uint) Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
update_movementflag = true;
}
}
catch (Exception e)
{
//Avoid system crash, can be slower but...
MainConsole.Instance.DebugFormat("Crash! {0}", e);
}
}
}
}
// Cause the avatar to stop flying if it's colliding
// with something with the down arrow pressed.
// Only do this if we're flying
if (m_physicsActor != null && m_physicsActor.Flying && !m_forceFly)
{
// Landing detection code
// Are the landing controls requirements filled?
bool controlland = (((flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0) ||
((flags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_UP_NEG) != 0));
// Applies a satisfying roll effect to the avatar when flying.
if (((flags & AgentManager.ControlFlags.AGENT_CONTROL_TURN_LEFT) != 0) &&
((flags & AgentManager.ControlFlags.AGENT_CONTROL_YAW_POS) != 0))
{
ApplyFlyingRoll(FLY_ROLL_RADIANS_PER_UPDATE,
((flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_POS) != 0),
((flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0));
}
else if (((flags & AgentManager.ControlFlags.AGENT_CONTROL_TURN_RIGHT) != 0) &&
((flags & AgentManager.ControlFlags.AGENT_CONTROL_YAW_NEG) != 0))
{
ApplyFlyingRoll(-FLY_ROLL_RADIANS_PER_UPDATE,
((flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_POS) != 0),
((flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0));
}
else
{
if (m_AngularVelocity.Z != 0)
m_AngularVelocity.Z += CalculateFlyingRollResetToZero(FLY_ROLL_RESET_RADIANS_PER_UPDATE);
}
// Are the collision requirements fulfilled?
bool colliding = (m_physicsActor.IsColliding == true);
if (m_physicsActor.Flying && colliding && controlland)
{
// nesting this check because LengthSquared() is expensive and we don't
// want to do it every step when flying.
// then call it in the if...
//The == Zero and Z > 0.1 are to stop people from flying and then falling down because the physics engine hasn't calculted the push yet
if (Velocity != Vector3.Zero && Math.Abs(Velocity.Z) > 0.05 &&
(Velocity.LengthSquared() <= LAND_VELOCITYMAG_MAX))
{
StopFlying();
SendTerseUpdateToAllClients();
}
}
}
// If the agent update does move the avatar, then calculate the force ready for the velocity update,
// which occurs later in the main scene loop
if (update_movementflag || (update_rotation && DCFlagKeyPressed))
{
// MainConsole.Instance.DebugFormat("{0} {1}", update_movementflag, (update_rotation && DCFlagKeyPressed));
// MainConsole.Instance.DebugFormat(
// "In {0} adding velocity to {1} of {2}", m_scene.RegionInfo.RegionName, Name, agent_control_v3);
AddNewMovement(agent_control_v3, q);
}
}
if ((update_movementflag || update_rotation) && (m_parentID == UUID.Zero))
Animator.UpdateMovementAnimations(false);
IAgentUpdateMonitor reporter =
m_scene.RequestModuleInterface<IMonitorModule>().GetMonitor<IAgentUpdateMonitor>(Scene);
if (reporter != null)
reporter.AddAgentTime(Util.EnvironmentTickCountSubtract(m_perfMonMS));
}
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
_parent_scene.AddSimulationChange(() => changeforce(force, pushforce));
}
else
{
MainConsole.Instance.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
}
//MainConsole.Instance.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
}
/// <summary>
/// ODE Avatar.
/// </summary>
/// <param name="avName"></param>
/// <param name="parent_scene"></param>
/// <param name="pos"></param>
/// <param name="size"></param>
/// <param name="pid_d"></param>
/// <param name="pid_p"></param>
/// <param name="capsule_radius"></param>
/// <param name="tensor"></param>
/// <param name="density">
/// Only used right now to return information to LSL. Not actually used to set mass in ODE!
/// </param>
/// <param name="walk_divisor"></param>
/// <param name="rundivisor"></param>
public OdeCharacter(
String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p,
float capsule_radius, float tensor, float density,
float walk_divisor, float rundivisor)
{
m_uuid = UUID.Random();
if (pos.IsFinite())
{
if (pos.Z > 9999999f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
if (pos.Z < -90000f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
_position = pos;
m_taintPosition = pos;
}
else
{
_position
= new Vector3(
(float)_parent_scene.WorldExtents.X * 0.5f,
(float)_parent_scene.WorldExtents.Y * 0.5f,
parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
m_taintPosition = _position;
m_log.WarnFormat("[ODE CHARACTER]: Got NaN Position on Character Create for {0}", avName);
}
_parent_scene = parent_scene;
PID_D = pid_d;
PID_P = pid_p;
CAPSULE_RADIUS = capsule_radius;
m_tensor = tensor;
m_density = density;
// heightFudgeFactor = height_fudge_factor;
walkDivisor = walk_divisor;
runDivisor = rundivisor;
// m_StandUpRotation =
// new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
// 0.5f);
// We can set taint and actual to be the same here, since the entire character will be set up when the
// m_tainted_isPhysical is processed.
SetTaintedCapsuleLength(size);
CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
_parent_scene.AddPhysicsActorTaint(this);
Name = avName;
}
public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
{
Name = primName;
m_vehicle = new ODEDynamics();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
ode = dode;
if (!pos.IsFinite())
{
pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
}
_position = pos;
m_taintposition = pos;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
m_density = parent_scene.geomDefaultDensity;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
if (!pos.IsFinite())
{
size = new Vector3(0.5f, 0.5f, 0.5f);
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
}
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
m_taintsize = _size;
if (!QuaternionIsFinite(rotation))
{
rotation = Quaternion.Identity;
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
}
_orientation = rotation;
m_taintrot = _orientation;
_pbs = pbs;
_parent_scene = parent_scene;
m_targetSpace = (IntPtr)0;
if (pos.Z < 0)
{
IsPhysical = false;
}
else
{
IsPhysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
if (IsPhysical)
m_targetSpace = _parent_scene.space;
}
m_taintadd = true;
_parent_scene.AddPhysicsActorTaint(this);
// don't do .add() here; old geoms get recycled with the same hash
}
/// <summary>
/// Adds the force supplied to the Target Velocity
/// The PID controller takes this target velocity and tries to make it a reality
/// </summary>
/// <param name="force"></param>
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
if (pushforce)
{
m_pidControllerActive = false;
force *= 100f;
m_taintForce += force;
_parent_scene.AddPhysicsActorTaint(this);
// If uncommented, things get pushed off world
//
// m_log.Debug("Push!");
// m_taintTargetVelocity.X += force.X;
// m_taintTargetVelocity.Y += force.Y;
// m_taintTargetVelocity.Z += force.Z;
}
else
{
m_pidControllerActive = true;
m_taintTargetVelocity += force;
}
}
else
{
m_log.WarnFormat("[ODE CHARACTER]: Got a NaN force applied to {0}", Name);
}
//m_lastUpdateSent = false;
}
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
lock (m_forcelist)
m_forcelist.Add(force);
m_taintforce = true;
}
else
{
m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
}
//m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
}
public override void AddForce(Vector3 force, bool pushforce) {
if (force.IsFinite())
{
_force.X += force.X;
_force.Y += force.Y;
_force.Z += force.Z;
// m_log.DebugFormat("{0}: AddForce. adding={1}, newForce={2}", LogHeader, force, _force);
_scene.TaintedObject(delegate()
{
BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
});
}
else
{
m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader);
}
//m_lastUpdateSent = false;
}
public override void setAvatarSize(Vector3 size, float feetOffset)
{
if (size.IsFinite())
{
if (size.X < 0.01f)
size.X = 0.01f;
if (size.Y < 0.01f)
size.Y = 0.01f;
if (size.Z < 0.01f)
size.Z = 0.01f;
strAvatarSize st = new strAvatarSize();
st.size = size;
st.offset = feetOffset;
AddChange(changes.AvatarSize, st);
}
else
{
m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character");
}
}