/// <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"); } }