C# Vector3.IsFinite方法代码示例

 /// <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)
                {
//.........这里部分代码省略.........

        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;
//.........这里部分代码省略.........

        public AuroraODEPrim(String primName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Vector3 size,
                       Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, float Density)
        {
            m_vehicle = new AuroraODEDynamics();
            //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
            ode = dode;
            if (!pos.IsFinite())
            {
                pos = new Vector3((parent_scene.Region.RegionSizeX * 0.5f), (parent_scene.Region.RegionSizeY * 0.5f),
                    parent_scene.GetTerrainHeightAtXY((parent_scene.Region.RegionSizeX * 0.5f), (parent_scene.Region.RegionSizeY * 0.5f)));
                m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
            }
            _position = pos;
            fakepos = false;

            PID_D = parent_scene.bodyPIDD;
            PID_G = parent_scene.bodyPIDG;

            // correct for changed timestep
            PID_D /= (parent_scene.ODE_STEPSIZE * 50f); // original ode fps of 50
            PID_G /= (parent_scene.ODE_STEPSIZE * 50f);

            m_density = Density / 100;
            // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;


            prim_geom = IntPtr.Zero;
            prev_geom = IntPtr.Zero;

            if (!size.IsFinite())
            {
                size = new Vector3(0.5f, 0.5f, 0.5f);
                m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
            }

            if (size.X <= 0) size.X = 0.01f;
            if (size.Y <= 0) size.Y = 0.01f;
            if (size.Z <= 0) size.Z = 0.01f;

            _size = size;

            if (!QuaternionIsFinite(rotation))
            {
                rotation = Quaternion.Identity;
                m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
            }

            _orientation = rotation;
            fakeori = false;

            _mesh = mesh;
            _pbs = pbs;

            _parent_scene = parent_scene;
            m_targetSpace = (IntPtr)0;

            if (pos.Z < 0)
                m_isphysical = false;
            else
            {
                m_isphysical = pisPhysical;
                // If we're physical, we need to be in the master space for now.
                // linksets *should* be in a space together..  but are not currently
                if (m_isphysical)
                    m_targetSpace = _parent_scene.space;
            }
            m_primName = primName;

            m_forceacc = Vector3.Zero;
            m_angularforceacc = Vector3.Zero;

            m_UpdateTimecntr = 0;
            m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
            if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
                m_UpdateTimecntr = .1f;

            AddChange(changes.Add, null);
        }

 public override void AddAngularForce(Vector3 force, bool pushforce)
 {
     if (force.IsFinite())
     {
         AddChange(changes.AddAngForce, (object) force);
     }
     else
     {
         m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
     }
 }

 /// <summary>
 /// Adds the force supplied to the Target Velocity
 /// The PID controller takes this target velocity and tries to make it a reality
 /// </summary>
 /// <param name="force"></param>
 public override void AddForce(Vector3 force, bool pushforce)
 {
     if (force.IsFinite())
     {
         if (pushforce)
         {
             m_pidControllerActive = false;
             force *= 100f;
             doForce(force);
             // If uncommented, things get pushed off world
             //
             // m_log.Debug("Push!");
             // _target_velocity.X += force.X;
             // _target_velocity.Y += force.Y;
             // _target_velocity.Z += force.Z;
         }
         else
         {
             m_pidControllerActive = true;
             _target_velocity.X += force.X;
             _target_velocity.Y += force.Y;
             _target_velocity.Z += force.Z;
         }
     }
     else
     {
         m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
     }
     //m_lastUpdateSent = false;
 }

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

            // If the PID Controller isn't active then we set our force
            // calculating base velocity to the current position

            if (Body == IntPtr.Zero)
                return;

            if (m_pidControllerActive == false)
            {
                _zeroPosition = d.BodyGetPosition(Body);
            }
            //PidStatus = true;

            d.Vector3 localpos = d.BodyGetPosition(Body);
            Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
            
            if (!localPos.IsFinite())
            {

                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
                defects.Add(this);
                // _parent_scene.RemoveCharacter(this);

                // destroy avatar capsule and related ODE data
                if (Amotor != IntPtr.Zero)
                {
                    // Kill the Amotor
                    d.JointDestroy(Amotor);
                    Amotor = IntPtr.Zero;
                }

                //kill the Geometry
                _parent_scene.waitForSpaceUnlock(_parent_scene.space);

                if (Body != IntPtr.Zero)
                {
                    //kill the body
                    d.BodyDestroy(Body);

                    Body = IntPtr.Zero;
                }

                if (Shell != IntPtr.Zero)
                {
                    d.GeomDestroy(Shell);
                    _parent_scene.geom_name_map.Remove(Shell);
                    Shell = IntPtr.Zero;
                }

                return;
            }

            Vector3 vec = Vector3.Zero;
            d.Vector3 vel = d.BodyGetLinearVel(Body);

            float movementdivisor = 1f;

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

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

                    d.Vector3 pos = d.BodyGetPosition(Body);
                    vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
                    vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
                    if (flying)
                    {
                        vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
                    }
                }
                //PidStatus = true;
            }
//.........这里部分代码省略.........

        /// <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

//.........这里部分代码省略.........

        public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
        {
            m_uuid = UUID.Random();

            if (pos.IsFinite())
            {
                if (pos.Z > 9999999f)
                {
                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                }
                if (pos.Z < -90000f)
                {
                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                }
                _position = pos;
                m_taintPosition.X = pos.X;
                m_taintPosition.Y = pos.Y;
                m_taintPosition.Z = pos.Z;
            }
            else
            {
                _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
                m_taintPosition.X = _position.X;
                m_taintPosition.Y = _position.Y;
                m_taintPosition.Z = _position.Z;
                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
            }

            _parent_scene = parent_scene;

            PID_D = pid_d;
            PID_P = pid_p;
            CAPSULE_RADIUS = capsule_radius;
            m_tensor = tensor;
            m_density = density;
            heightFudgeFactor = height_fudge_factor;
            walkDivisor = walk_divisor;
            runDivisor = rundivisor;

            // m_StandUpRotation =
            //     new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
            //                   0.5f);

            for (int i = 0; i < 11; i++)
            {
                m_colliderarr[i] = false;
            }
            CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
            //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
            m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;

            m_isPhysical = false; // current status: no ODE information exists
            m_tainted_isPhysical = true; // new tainted status: need to create ODE information

            _parent_scene.AddPhysicsActorTaint(this);
            
            m_name = avName;
        }

        public AuroraODECharacter(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation, Vector3 size)
        {
            m_uuid = UUID.Random();

            m_taintRotation = rotation;
//            _orientation = rotation;
//            _lastorientation = rotation;

            if (pos.IsFinite())
            {
                if (pos.Z > 9999999f || pos.Z <-90f)
                {
//                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                    pos.Z = parent_scene.GetTerrainHeightAtXY(parent_scene.Region.RegionSizeX * 0.5f, parent_scene.Region.RegionSizeY * 0.5f) + 5.0f;
                }
                _position = pos;
                m_taintPosition.X = pos.X;
                m_taintPosition.Y = pos.Y;
                m_taintPosition.Z = pos.Z;
            }
            else
            {
                _position.X = (float)parent_scene.Region.RegionSizeX * 0.5f;
                _position.Y = (float)parent_scene.Region.RegionSizeY * 0.5f;
                _position.Z = parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + 10f;
                    
                m_taintPosition.X = _position.X;
                m_taintPosition.Y = _position.Y;
                m_taintPosition.Z = _position.Z;
                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
            }

            _parent_scene = parent_scene;

            CAPSULE_RADIUS = parent_scene.avCapRadius;

            // m_StandUpRotation =
            //     new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
            //                   0.5f);

            CAPSULE_LENGTH = (size.Z * 1.1f) - CAPSULE_RADIUS * 2.0f;

            if ((m_collisionFlags & CollisionCategories.Land) == 0)
                AvatarHalfsize = CAPSULE_LENGTH * 0.5f + CAPSULE_RADIUS;
            else
                AvatarHalfsize = CAPSULE_LENGTH * 0.5f + CAPSULE_RADIUS - 0.3f;

            //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
            m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;

            m_isPhysical = false; // current status: no ODE information exists
            m_tainted_isPhysical = true; // new tainted status: need to create ODE information

            _parent_scene.AddPhysicsActorTaint(this);

            m_UpdateTimecntr = 0;
            m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
            if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
                m_UpdateTimecntr = .1f;
            m_name = avName;
        }

        /// <summary>
        /// Called from Simulate
        /// This is the avatar's movement control + PID Controller
        /// </summary>
        /// <param name="timeStep"></param>
        public void Move(List<OdeCharacter> defects)
        {
            if (Body == IntPtr.Zero)
                return;

            d.Vector3 dtmp = d.BodyGetPosition(Body);
            Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);

            // the Amotor still lets avatar rotation to drift during colisions
            // so force it back to identity

            d.Quaternion qtmp;
            qtmp.W = m_orientation2D.W;
            qtmp.X = m_orientation2D.X;
            qtmp.Y = m_orientation2D.Y;
            qtmp.Z = m_orientation2D.Z;
            d.BodySetQuaternion(Body, ref qtmp);

            if (m_pidControllerActive == false)
            {
                _zeroPosition = localpos;
            }

            if (!localpos.IsFinite())
            {
                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
                defects.Add(this);
                // _parent_scene.RemoveCharacter(this);

                // destroy avatar capsule and related ODE data
                AvatarGeomAndBodyDestroy();
                return;
            }

            // check outbounds forcing to be in world
            bool fixbody = false;
            if (localpos.X < 0.0f)
            {
                fixbody = true;
                localpos.X = 0.1f;
            }
            else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
            {
                fixbody = true;
                localpos.X = _parent_scene.WorldExtents.X - 0.1f;
            }
            if (localpos.Y < 0.0f)
            {
                fixbody = true;
                localpos.Y = 0.1f;
            }
            else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
            {
                fixbody = true;
                localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
            }
            if (fixbody)
            {
                m_freemove = false;
                d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
            }

            float breakfactor;

            Vector3 vec = Vector3.Zero;
            dtmp = d.BodyGetLinearVel(Body);
            Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
            float velLengthSquared = vel.LengthSquared();

            Vector3 ctz = _target_velocity;

            float movementdivisor = 1f;
            //Ubit change divisions into multiplications below
            if (!m_alwaysRun)
                movementdivisor = 1 / walkDivisor;
            else
                movementdivisor = 1 / runDivisor;

            ctz.X *= movementdivisor;
            ctz.Y *= movementdivisor;

            //******************************************
            // colide with land

            d.AABB aabb;
//            d.GeomGetAABB(feetbox, out aabb);
            d.GeomGetAABB(capsule, out aabb);
            float chrminZ = aabb.MinZ; // move up a bit
            Vector3 posch = localpos;

            float ftmp;

            if (flying)
            {
                ftmp = timeStep;
//.........这里部分代码省略.........

 public override void SetMomentum(Vector3 momentum)
 {
     if (momentum.IsFinite())
         AddChange(changes.Momentum, momentum);
 }

 public override void AddAngularForce(Vector3 force, bool pushforce)
 {
     if (force.IsFinite())
     {
         m_angularforcelist.Add(force);
         m_taintaddangularforce = true;
     }
     else
     {
         m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
     }
 }