OpenSim.Region.Physics.BulletDotNETPlugin.BulletDotNETScene.GetTerrainHeightAtXY C# (CSharp) Method

        public float GetTerrainHeightAtXY(float x, float y)
        {
            // Teravus: Kitto, this code causes recurring errors that stall physics permenantly unless 
            // the values are checked, so checking below.
            // Is there any reason that we don't do this in ScenePresence?
            // The only physics engine that benefits from it in the physics plugin is this one

            if (x > (int)Constants.RegionSize || y > (int)Constants.RegionSize ||
                x < 0.001f || y < 0.001f)
                return 0;

            return _origheightmap[(int)y * Constants.RegionSize + (int)x];
        }
        // End recovered. Kitto Flora

        /// <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());

            vel *= 0.25f;

            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) * 10;
                }
            }
            if (m_flying)
            {
                // Slight PID correction
                vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 1.5f);


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