C# AuroraODEPhysicsScene.GetTerrainHeightAtXY方法代码示例

        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 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;
        }

        public AuroraODEPrim(String primName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Vector3 size,
                       Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
        {
            m_vehicle = new AuroraODEDynamics();
            //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.Warn("[PHYSICS]: Got nonFinite Object create Position");
            }
            _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;
            prev_geom = IntPtr.Zero;

            if (!pos.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;
            m_taintsize = _size;

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

            _orientation = rotation;
            m_taintrot = _orientation;
            _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_taintadd = true;
            _parent_scene.AddPhysicsActorTaint(this);
            //  don't do .add() here; old geoms get recycled with the same hash
        }

        }   // end Step

        private void MoveLinear(float pTimestep, AuroraODEPhysicsScene _pParentScene)
        {
            d.Vector3 pos = d.BodyGetPosition (Body);
            d.Vector3 oldPos = pos;

            if (m_lastPositionVector.X != pos.X ||
                m_lastPositionVector.Y != pos.Y ||
                m_lastPositionVector.Z != pos.Z)
            {
                m_lastPositionVector = d.BodyGetPosition (Body);
                m_lastAngularVelocity = new Vector3 ((float)d.BodyGetAngularVel (Body).X, (float)d.BodyGetAngularVel (Body).Y, (float)d.BodyGetAngularVel (Body).Z);
            }
            if (!m_linearMotorDirection.ApproxEquals (Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
            {
                if (!d.BodyIsEnabled (Body))
                    d.BodyEnable (Body);

                // add drive to body
                Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale * m_linearMotorDecayTimescale / (pTimestep));
                
                m_lastLinearVelocityVector += (addAmount * 10);  // lastLinearVelocityVector is the current body velocity vector?

                // This will work temporarily, but we really need to compare speed on an axis
                // KF: Limit body velocity to applied velocity?
                if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
                    m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
                if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
                    m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
            }
            else
            {        // requested is not significant
                // if what remains of applied is small, zero it.
                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
                    m_lastLinearVelocityVector = Vector3.Zero;
            }
            m_linearMotorDirection = Vector3.Zero;
            // convert requested object velocity to world-referenced vector
            m_dir = m_lastLinearVelocityVector;
            d.Quaternion rot = d.BodyGetQuaternion (Body);
            Quaternion rotq = new Quaternion (rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
            m_dir *= rotq;

            // Preserve the current Z velocity
            d.Vector3 vel_now = d.BodyGetLinearVel(Body);
            m_dir.Z += (float)vel_now.Z;        // Preserve the accumulated falling velocity

            #region Blocking End Points

            //This makes sure that the vehicle doesn't leave the defined limits of position
            if (m_BlockingEndPoint != Vector3.Zero)
            {
                Vector3 posChange = new Vector3();
                posChange.X = (float)(pos.X - m_lastPositionVector.X);
                posChange.Y = (float)(pos.Y - m_lastPositionVector.Y);
                posChange.Z = (float)(pos.Z - m_lastPositionVector.Z);

                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                    pos.X -= posChange.X + 1;

                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
                    pos.Y -= posChange.Y + 1;

                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
                    pos.Z -= posChange.Z + 1;

                if (pos.X <= 0)
                    pos.X += posChange.X + 1;

                if (pos.Y <= 0)
                    pos.Y += posChange.Y + 1;
            }

            #endregion

            // Check if hovering
            if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
            {
                // We should hover, get the target height
                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                {
                    m_VhoverTargetHeight = (float)_pParentScene.GetWaterLevel((float)pos.X, (float)pos.Y) + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                {
                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY((float)pos.X, (float)pos.Y) + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                {
                    m_VhoverTargetHeight = m_VhoverHeight;
                }

                if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
                {
                    // If body is already heigher, use its height as target height
                    if (pos.Z > m_VhoverTargetHeight)
                        m_VhoverTargetHeight = (float)pos.Z;
//.........这里部分代码省略.........

        public AuroraODECharacter(String avName, AuroraODEPhysicsScene 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");
            }


            CAPSULE_RADIUS = _parent_scene.avCapRadius;
            CAPSULE_LENGTH = (size.Z*1.1f) - CAPSULE_RADIUS*2.0f;
            AvatarHalfsize = CAPSULE_LENGTH*0.5f + CAPSULE_RADIUS;

            m_isPhysical = false; // current status: no ODE information exists
            _parent_scene.AddSimulationChange(() => RebuildAvatar());
            m_name = avName;
        }

        // end Step

        private void MoveLinear(float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
        {
            d.Vector3 dvel_now = d.BodyGetLinearVel(Body);

            if (m_linearMotorDirection.LengthSquared() < 0.0001f)
            {
                m_linearMotorDirection = Vector3.Zero;
                m_newVelocity = Vector3.Zero;
            }
            else
            {
                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/m_linearMotorTimescale;
                m_lastLinearVelocityVector += (addAmount);

                m_linearMotorDirection *= (1.0f - 1.0f/m_linearMotorDecayTimescale);

                // convert requested object velocity to world-referenced vector
                d.Quaternion rot = d.BodyGetQuaternion(Body);
                Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
                m_newVelocity = m_lastLinearVelocityVector;
                m_newVelocity *= rotq; // apply obj rotation to velocity vector
            }

            if (m_newVelocity.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
                m_newVelocity.Z += dvel_now.Z; // Preserve the accumulated falling velocity

            d.Vector3 dpos = d.BodyGetPosition(Body);
            Vector3 pos = new Vector3(dpos.X, dpos.Y, dpos.Z);

            if (!(m_lastPositionVector.X == 0 &&
                  m_lastPositionVector.Y == 0 &&
                  m_lastPositionVector.Z == 0))
            {
                ///Only do this if we have a last position
                m_lastposChange.X = pos.X - m_lastPositionVector.X;
                m_lastposChange.Y = pos.Y - m_lastPositionVector.Y;
                m_lastposChange.Z = pos.Z - m_lastPositionVector.Z;
            }

            #region Blocking Change

            double Zchange = Math.Abs(m_lastposChange.Z);

            if (m_BlockingEndPoint != Vector3.Zero)
            {
                bool needUpdateBody = false;
                if (pos.X >= (m_BlockingEndPoint.X - 1))
                {
                    pos.X -= m_lastposChange.X + 1;
                    needUpdateBody = true;
                }
                if (pos.Y >= (m_BlockingEndPoint.Y - 1))
                {
                    pos.Y -= m_lastposChange.Y + 1;
                    needUpdateBody = true;
                }
                if (pos.Z >= (m_BlockingEndPoint.Z - 1))
                {
                    pos.Z -= m_lastposChange.Z + 1;
                    needUpdateBody = true;
                }
                if (pos.X <= 0)
                {
                    pos.X += m_lastposChange.X + 1;
                    needUpdateBody = true;
                }
                if (pos.Y <= 0)
                {
                    pos.Y += m_lastposChange.Y + 1;
                    needUpdateBody = true;
                }
                if (needUpdateBody)
                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }

            #endregion

            #region Terrain checks

            float terrainHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
            if (pos.Z < terrainHeight - 5)
            {
                pos.Z = terrainHeight + 2;
                m_lastPositionVector = pos;
                    //Make sure that we don't have an explosion the next frame with the posChange
                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }
            else if (pos.Z < terrainHeight)
            {
                m_newVelocity.Z += 1;
            }

            #endregion

            #region Hover

            // Check if hovering
            if ((m_flags &
//.........这里部分代码省略.........

        public AuroraODECharacter(String avName, AuroraODEPhysicsScene 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;
            m_name = avName;
        }

//.........这里部分代码省略.........
            if (m_BlockingEndPoint != Vector3.Zero)
            {
                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                {
                    pos.X -= m_lastposChange.X + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
                {
                    pos.Y -= m_lastposChange.Y + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
                {
                    pos.Z -= m_lastposChange.Z + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.X <= 0)
                {
                    pos.X += m_lastposChange.X + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Y <= 0)
                {
                    pos.Y += m_lastposChange.Y + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
            }

            #endregion

            #region Terrain checks

            float terrainHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
            if(pos.Z < terrainHeight - 5)
            {
                pos.Z = terrainHeight + 2;
                m_lastPositionVector = pos;//Make sure that we don't have an explosion the next frame with the posChange
                d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
            }
            else if(pos.Z < terrainHeight)
            {
                m_dir.Z += 1;
            }

            #endregion

            #region Hover

            // Check if hovering
            if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
            {
                // We should hover, get the target height
                if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                {
                    m_VhoverTargetHeight = (float)_pParentScene.GetWaterLevel (pos.X, pos.Y) + m_VhoverHeight;
                }
                if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                {
                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY (pos.X, pos.Y) + m_VhoverHeight;
                }
                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                {
                    m_VhoverTargetHeight = m_VhoverHeight;
                }

        public AuroraODECharacter(String avName, AuroraODEPhysicsScene 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.1f) - 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;
        }

        }   // end Step

        private void MoveLinear (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
        {
            if (!m_linearMotorDirection.ApproxEquals (Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
            {
                if (!d.BodyIsEnabled (Body))
                    d.BodyEnable (Body);

                // add drive to body
                Vector3 addAmount = m_linearMotorDirection / ((m_linearMotorTimescale) / pTimestep);
                m_lastLinearVelocityVector += (addAmount);  // lastLinearVelocityVector is the current body velocity vector?

                // This will work temporarily, but we really need to compare speed on an axis
                // KF: Limit body velocity to applied velocity?
                if (Math.Abs (m_lastLinearVelocityVector.X) > Math.Abs (m_linearMotorDirectionLASTSET.X))
                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
                if (Math.Abs (m_lastLinearVelocityVector.Y) > Math.Abs (m_linearMotorDirectionLASTSET.Y))
                    m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
                if (Math.Abs (m_lastLinearVelocityVector.Z) > Math.Abs (m_linearMotorDirectionLASTSET.Z))
                    m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;

                // decay applied velocity
                Vector3 decayfraction = ((Vector3.One / (m_linearMotorDecayTimescale / (pTimestep))));
                decayfraction.Z = ((1 / (m_linearMotorDecayTimescale / (pTimestep * pTimestep))));
                //Console.WriteLine("decay: " + decayfraction);
                Vector3 decayAmt = (m_linearMotorDirection * decayfraction);
                m_linearMotorDirection -= decayAmt;
                //Console.WriteLine("actual: " + m_linearMotorDirection);
            }
            else
            {        // requested is not significant
                // if what remains of applied is small, zero it.
                if (m_lastLinearVelocityVector.ApproxEquals (Vector3.Zero, 0.01f))
                    m_lastLinearVelocityVector = Vector3.Zero;
            }

            // convert requested object velocity to world-referenced vector
            m_dir = m_lastLinearVelocityVector;
            d.Quaternion rot = d.BodyGetQuaternion (Body);
            Quaternion rotq = new Quaternion (rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
            m_dir *= rotq;                            // apply obj rotation to velocity vector

            // add Gravity andBuoyancy
            // KF: So far I have found no good method to combine a script-requested
            // .Z velocity and gravity. Therefore only 0g will used script-requested
            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
            Vector3 grav = Vector3.Zero;
            // There is some gravity, make a gravity force vector
            // that is applied after object velocity.
            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
            grav.Z = _pParentScene.gravityz * Mass * (float)parent.ParentEntity.GravityMultiplier * (1f - m_VehicleBuoyancy);
            // Preserve the current Z velocity
            d.Vector3 vel_now = d.BodyGetLinearVel (Body);
            if(m_lastLinearVelocityVector.Z == 0 && m_verticalAttractionTimescale == 0)
                m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
            else if(Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON)
                m_dir.Z += vel_now.Z;

            d.Vector3 pos = d.BodyGetPosition (Body);
            //            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
            Vector3 posChange = new Vector3 ();
            posChange.X = pos.X - m_lastPositionVector.X;
            posChange.Y = pos.Y - m_lastPositionVector.Y;
            posChange.Z = pos.Z - m_lastPositionVector.Z;
            double Zchange = Math.Abs (posChange.Z);
            if (m_BlockingEndPoint != Vector3.Zero)
            {
                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                {
                    pos.X -= posChange.X + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
                {
                    pos.Y -= posChange.Y + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
                {
                    pos.Z -= posChange.Z + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.X <= 0)
                {
                    pos.X += posChange.X + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Y <= 0)
                {
                    pos.Y += posChange.Y + 1;
                    d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
                }
            }
            if (pos.Z < _pParentScene.GetTerrainHeightAtXY (pos.X, pos.Y))
            {
                pos.Z = _pParentScene.GetTerrainHeightAtXY (pos.X, pos.Y) + 2;
                d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
            }

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