Exemple #1
0
 /// <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);
            }
        }
Exemple #4
0
        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
        }
Exemple #6
0
        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);
        }
Exemple #7
0
 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;
                }
            }
        }
Exemple #10
0
        /// <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;
        }
Exemple #12
0
        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;
        }
Exemple #13
0
        /// <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);
            }
        }
Exemple #14
0
 public override void SetMomentum(Vector3 momentum)
 {
     if (momentum.IsFinite())
         AddChange(changes.Momentum, momentum);
 }
Exemple #15
0
 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);
     }
 }
Exemple #16
0
        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;
        }
Exemple #17
0
 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);
     }
 }
Exemple #18
0
 /// <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;
 }
Exemple #19
0
 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());
 }
Exemple #20
0
 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");
     }
 }
Exemple #21
0
 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");
     }
 }
Exemple #22
0
 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));
        }
Exemple #24
0
 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;
        }
Exemple #26
0
        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;
        }
Exemple #28
0
        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());
        }
Exemple #29
0
 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;
 }
Exemple #30
0
        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");
            }
            
        }