/// <summary>
        /// Called from Simulate
        /// This is the avatar's movement control + PID Controller
        /// </summary>
        /// <param name="timeStep"></param>
        public void Move(float timeStep)
        {
            //  no lock; for now it's only called from within Simulate()

            // If the PID Controller isn't active then we set our force
            // calculating base velocity to the current position
            if (Body == null)
                return;
            tempTrans1.Dispose();
            tempTrans1 = Body.getInterpolationWorldTransform();
            tempVector1.Dispose();
            tempVector1 = tempTrans1.getOrigin();
            tempVector2.Dispose();
            tempVector2 = Body.getInterpolationLinearVelocity();

            if (m_pidControllerActive == false)
            {
                m_zeroPosition.X = tempVector1.getX();
                m_zeroPosition.Y = tempVector1.getY();
                m_zeroPosition.Z = tempVector1.getZ();
            }
            //PidStatus = true;

            Vector3 vec = Vector3.Zero;

            Vector3 vel = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());

            float movementdivisor = 1f;

            if (!m_alwaysRun)
            {
                movementdivisor = walkDivisor;
            }
            else
            {
                movementdivisor = runDivisor;
            }

            //  if velocity is zero, use position control; otherwise, velocity control
            if (m_target_velocity.X == 0.0f && m_target_velocity.Y == 0.0f && m_target_velocity.Z == 0.0f && m_iscolliding)
            {
                //  keep track of where we stopped.  No more slippin' & slidin'
                if (!m_zeroFlag)
                {
                    m_zeroFlag = true;
                    m_zeroPosition.X = tempVector1.getX();
                    m_zeroPosition.Y = tempVector1.getY();
                    m_zeroPosition.Z = tempVector1.getZ();
                }
                if (m_pidControllerActive)
                {
                    // We only want to deactivate the PID Controller if we think we want to have our surrogate
                    // react to the physics scene by moving it's position.
                    // Avatar to Avatar collisions
                    // Prim to avatar collisions

                    Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
                    vec.X = (m_target_velocity.X - vel.X) * (PID_D) + (m_zeroPosition.X - pos.X) * (PID_P * 2);
                    vec.Y = (m_target_velocity.Y - vel.Y) * (PID_D) + (m_zeroPosition.Y - pos.Y) * (PID_P * 2);
                    if (m_flying)
                    {
                        vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) + (m_zeroPosition.Z - pos.Z) * PID_P;
                    }
                }
                //PidStatus = true;
            }
            else
            {
                m_pidControllerActive = true;
                m_zeroFlag = false;
                if (m_iscolliding && !m_flying)
                {
                    // We're standing on something
                    vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
                    vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
                }
                else if (m_iscolliding && m_flying)
                {
                    // We're flying and colliding with something
                    vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16);
                    vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16);
                }
                else if (!m_iscolliding && m_flying)
                {
                    // we're in mid air suspended
                    vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6);
                    vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6);

                    // We don't want linear velocity to cause our avatar to bounce, so we check target Z and actual velocity X, Y
                    // rebound preventing
                    if (m_target_velocity.Z < 0.025f && m_velocity.X < 0.25f && m_velocity.Y < 0.25f)
                        m_zeroFlag = true;
                }

                if (m_iscolliding && !m_flying && m_target_velocity.Z > 0.0f)
                {
                    // We're colliding with something and we're not flying but we're moving
                    // This means we're walking or running.
                    Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
                    vec.Z = (m_target_velocity.Z - vel.Z) * PID_D + (m_zeroPosition.Z - pos.Z) * PID_P;
                    if (m_target_velocity.X > 0)
                    {
                        vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D;
                    }
                    if (m_target_velocity.Y > 0)
                    {
                        vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
                    }
                }
                else if (!m_iscolliding && !m_flying)
                {
                    // we're not colliding and we're not flying so that means we're falling!
                    // m_iscolliding includes collisions with the ground.

                    // d.Vector3 pos = d.BodyGetPosition(Body);
                    if (m_target_velocity.X > 0)
                    {
                        vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D;
                    }
                    if (m_target_velocity.Y > 0)
                    {
                        vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
                    }
                }


                if (m_flying)
                {
                    vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D);
                }
            }
            if (m_flying)
            {
                // Slight PID correction
                vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 0.06f);


                //auto fly height. Kitto Flora
                //d.Vector3 pos = d.BodyGetPosition(Body);
                float target_altitude = m_parent_scene.GetTerrainHeightAtXY(m_position.X, m_position.Y) + m_parent_scene.minimumGroundFlightOffset;

                if (m_position.Z < target_altitude)
                {
                    vec.Z += (target_altitude - m_position.Z) * PID_P * 5.0f;
                }

            }
            if (Body != null && (((m_target_velocity.X > 0.2f || m_target_velocity.X < -0.2f) || (m_target_velocity.Y > 0.2f || m_target_velocity.Y < -0.2f))))
            {
                Body.setFriction(0.001f);
                //m_log.DebugFormat("[PHYSICS]: Avatar force applied: {0}, Target:{1}", vec.ToString(), m_target_velocity.ToString());
            }

            if (Body != null)
            {
                int activationstate = Body.getActivationState();
                if (activationstate == 0)
                {
                    Body.forceActivationState(1);
                }
               

            }
            doImpulse(vec, true);
        }
示例#2
0
        public void UpdatePositionAndVelocity()
        {
            if (!m_isSelected)
            {
                if (_parent == null)
                {
                    Vector3 pv = Vector3.Zero;
                    bool lastZeroFlag = _zeroFlag;
                    if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
                        tempPosition3.Dispose();
                    if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
                        tempTransform3.Dispose();

                    if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
                        tempOrientation2.Dispose();

                    if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
                        tempAngularVelocity1.Dispose();

                    if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
                        tempLinearVelocity1.Dispose();



                    tempTransform3 = Body.getInterpolationWorldTransform();
                    tempPosition3 = tempTransform3.getOrigin(); // vec
                    tempOrientation2 = tempTransform3.getRotation(); // ori
                    tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel
                    tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel

                    _torque = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(),
                                      tempAngularVelocity1.getZ());
                    Vector3 l_position = Vector3.Zero;
                    Quaternion l_orientation = Quaternion.Identity;
                    m_lastposition = _position;
                    m_lastorientation = _orientation;

                    l_position.X = tempPosition3.getX();
                    l_position.Y = tempPosition3.getY();
                    l_position.Z = tempPosition3.getZ();
                    l_orientation.X = tempOrientation2.getX();
                    l_orientation.Y = tempOrientation2.getY();
                    l_orientation.Z = tempOrientation2.getZ();
                    l_orientation.W = tempOrientation2.getW();

                    if (l_position.X > ((int)Constants.RegionSize - 0.05f) || l_position.X < 0f || l_position.Y > ((int)Constants.RegionSize - 0.05f) || l_position.Y < 0f)
                    {
                        //base.RaiseOutOfBounds(l_position);

                        if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
                        {
                            _position = l_position;
                            //_parent_scene.remActivePrim(this);
                            if (_parent == null)
                                base.RequestPhysicsterseUpdate();
                            return;
                        }
                        else
                        {
                            if (_parent == null)
                                base.RaiseOutOfBounds(l_position);
                            return;
                        }
                    }

                    if (l_position.Z < -200000f)
                    {
                        // This is so prim that get lost underground don't fall forever and suck up
                        //
                        // Sim resources and memory.
                        // Disables the prim's movement physics....
                        // It's a hack and will generate a console message if it fails.

                        //IsPhysical = false;
                        //if (_parent == null)
                        //base.RaiseOutOfBounds(_position);

                        _acceleration.X = 0;
                        _acceleration.Y = 0;
                        _acceleration.Z = 0;

                        _velocity.X = 0;
                        _velocity.Y = 0;
                        _velocity.Z = 0;
                        m_rotationalVelocity.X = 0;
                        m_rotationalVelocity.Y = 0;
                        m_rotationalVelocity.Z = 0;

                        if (_parent == null)
                            base.RequestPhysicsterseUpdate();

                        m_throttleUpdates = false;
                        // throttleCounter = 0;
                        _zeroFlag = true;
                        //outofBounds = true;
                    }

                    if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
                        && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
                        && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
                        && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
                    {
                        _zeroFlag = true;
                        m_throttleUpdates = false;
                    }
                    else
                    {
                        //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
                        _zeroFlag = false;
                    }

                    if (_zeroFlag)
                    {
                        _velocity.X = 0.0f;
                        _velocity.Y = 0.0f;
                        _velocity.Z = 0.0f;

                        _acceleration.X = 0;
                        _acceleration.Y = 0;
                        _acceleration.Z = 0;

                        //_orientation.w = 0f;
                        //_orientation.X = 0f;
                        //_orientation.Y = 0f;
                        //_orientation.Z = 0f;
                        m_rotationalVelocity.X = 0;
                        m_rotationalVelocity.Y = 0;
                        m_rotationalVelocity.Z = 0;
                        if (!m_lastUpdateSent)
                        {
                            m_throttleUpdates = false;
                            // throttleCounter = 0;
                            m_rotationalVelocity = pv;

                            if (_parent == null)
                                base.RequestPhysicsterseUpdate();

                            m_lastUpdateSent = true;
                        }
                    }
                    else
                    {
                        if (lastZeroFlag != _zeroFlag)
                        {
                            if (_parent == null)
                                base.RequestPhysicsterseUpdate();
                        }

                        m_lastVelocity = _velocity;

                        _position = l_position;

                        _velocity.X = tempLinearVelocity1.getX();
                        _velocity.Y = tempLinearVelocity1.getY();
                        _velocity.Z = tempLinearVelocity1.getZ();

                        _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
                        _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
                                                          _velocity.Y - m_lastVelocity.Y / 0.1f,
                                                          _velocity.Z - m_lastVelocity.Z / 0.1f);
                        //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());

                        if (_velocity.ApproxEquals(pv, 0.5f))
                        {
                            m_rotationalVelocity = pv;
                        }
                        else
                        {
                            m_rotationalVelocity = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ());
                        }

                        //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());

                        _orientation.X = l_orientation.X;
                        _orientation.Y = l_orientation.Y;
                        _orientation.Z = l_orientation.Z;
                        _orientation.W = l_orientation.W;
                        m_lastUpdateSent = false;

                        //if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
                        //{
                        if (_parent == null)
                            base.RequestPhysicsterseUpdate();
                        // }
                        // else
                        // {
                        //     throttleCounter++;
                        //}

                    }
                    m_lastposition = l_position;
                    if (forceenable)
                    {
                        Body.forceActivationState(1);
                        forceenable = false;
                    }
                }
                else
                {
                    // Not a body..   so Make sure the client isn't interpolating
                    _velocity.X = 0;
                    _velocity.Y = 0;
                    _velocity.Z = 0;

                    _acceleration.X = 0;
                    _acceleration.Y = 0;
                    _acceleration.Z = 0;

                    m_rotationalVelocity.X = 0;
                    m_rotationalVelocity.Y = 0;
                    m_rotationalVelocity.Z = 0;
                    _zeroFlag = true;
                }
            }
        }
        /// <summary>
        /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
        /// </summary>
        public void UpdatePositionAndVelocity()
        {
            if (Body == null)
                return;
            //int val = Environment.TickCount;
            CheckIfStandingOnObject();
            //m_log.DebugFormat("time:{0}", Environment.TickCount - val);

            //IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody);
            
            tempTrans1.Dispose();
            tempTrans1 = Body.getInterpolationWorldTransform();
            tempVector1.Dispose();
            tempVector1 = tempTrans1.getOrigin();
            tempVector2.Dispose();
            tempVector2 = Body.getInterpolationLinearVelocity();
            
            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
            Vector3 vec = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());

            //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
            if (vec.X < -10.0f) vec.X = 0.0f;
            if (vec.Y < -10.0f) vec.Y = 0.0f;
            if (vec.X > (int)Constants.RegionSize + 10.2f) vec.X = (int)Constants.RegionSize + 10.2f;
            if (vec.Y > (int)Constants.RegionSize + 10.2f) vec.Y = (int)Constants.RegionSize + 10.2f;

            m_position.X = vec.X;
            m_position.Y = vec.Y;
            m_position.Z = vec.Z;

            // Did we move last? = zeroflag
            // This helps keep us from sliding all over

            if (m_zeroFlag)
            {
                m_velocity.X = 0.0f;
                m_velocity.Y = 0.0f;
                m_velocity.Z = 0.0f;

                // Did we send out the 'stopped' message?
                if (!m_lastUpdateSent)
                {
                    m_lastUpdateSent = true;
                    base.RequestPhysicsterseUpdate();

                }
            }
            else
            {
                m_lastUpdateSent = false;
                vec = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
                m_velocity.X = (vec.X);
                m_velocity.Y = (vec.Y);

                m_velocity.Z = (vec.Z);
                //m_log.Debug(m_target_velocity);
                if (m_velocity.Z < -6 && !m_hackSentFall)
                {
                    m_hackSentFall = true;
                    m_pidControllerActive = false;
                }
                else if (m_flying && !m_hackSentFly)
                {
                    //m_hackSentFly = true;
                    //base.SendCollisionUpdate(new CollisionEventUpdate());
                }
                else
                {
                    m_hackSentFly = false;
                    m_hackSentFall = false;
                }
            }
            if (Body != null)
            {
                if (Body.getFriction() < 0.9f)
                    Body.setFriction(0.9f);
            }
            //if (Body != null)
            //    Body.clearForces();
        }
        /// <summary>
        /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
        /// </summary>
        public void UpdatePositionAndVelocity()
        {
            if (Body == null)
                return;
            //int val = Environment.TickCount;
            CheckIfStandingOnObject();
            //m_log.DebugFormat("time:{0}", Environment.TickCount - val);

            //IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody);
            
            tempTrans1.Dispose();
            tempTrans1 = Body.getInterpolationWorldTransform();
            tempVector1.Dispose();
            tempVector1 = tempTrans1.getOrigin();
            tempVector2.Dispose();
            tempVector2 = Body.getInterpolationLinearVelocity();
            
            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
            Vector3 vec = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());

            //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
            if (vec.X < -10.0f) vec.X = 0.0f;
            if (vec.Y < -10.0f) vec.Y = 0.0f;
            if (vec.X > m_parent_scene.m_region.RegionSizeX + 10.2f) vec.X = m_parent_scene.m_region.RegionSizeX + 10.2f;
            if (vec.Y > m_parent_scene.m_region.RegionSizeY + 10.2f) vec.Y = m_parent_scene.m_region.RegionSizeY + 10.2f;

            m_position.X = vec.X;
            m_position.Y = vec.Y;
            m_position.Z = vec.Z;

            // Did we move last? = zeroflag
            // This helps keep us from sliding all over

            if (m_zeroFlag)
            {
                m_velocity.X = 0.0f;
                m_velocity.Y = 0.0f;
                m_velocity.Z = 0.0f;

                // Did we send out the 'stopped' message?
                if (!m_lastUpdateSent)
                {
                    m_lastUpdateSent = true;
                    base.RequestPhysicsterseUpdate();
                }

                //Tell any listeners that we've stopped
                base.TriggerMovementUpdate();
            }
            else
            {
                m_lastUpdateSent = false;
                vec = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
                m_velocity.X = (vec.X);
                m_velocity.Y = (vec.Y);

                m_velocity.Z = (vec.Z);
                //m_log.Debug(m_target_velocity);
                if (m_velocity.Z < -6 && !m_hackSentFall)
                {
                    m_hackSentFall = true;
                    m_pidControllerActive = false;
                }
                else if (m_flying && !m_hackSentFly)
                {
                    //m_hackSentFly = true;
                    //base.SendCollisionUpdate(new CollisionEventUpdate());
                }
                else
                {
                    m_hackSentFly = false;
                    m_hackSentFall = false;
                }
                const float VELOCITY_TOLERANCE = 0.001f;
                const float POSITION_TOLERANCE = 0.05f;

                //Check to see whether we need to trigger the significant movement method in the presence
                if (!RotationalVelocity.ApproxEquals(m_lastRotationalVelocity, VELOCITY_TOLERANCE) ||
                    !Velocity.ApproxEquals(m_lastVelocity, VELOCITY_TOLERANCE) ||
                    !Position.ApproxEquals(m_lastPosition, POSITION_TOLERANCE))
                {
                    // Update the "last" values
                    m_lastPosition = Position;
                    m_lastRotationalVelocity = RotationalVelocity;
                    m_lastVelocity = Velocity;
                    base.RequestPhysicsterseUpdate();
                    base.TriggerSignificantMovement();
                }
                //Tell any listeners about the new info
                base.TriggerMovementUpdate();
            }
            if (Body != null)
            {
                if (Body.getFriction() < 0.9f)
                    Body.setFriction(0.9f);
            }
            //if (Body != null)
            //    Body.clearForces();
        }