C# Vector3.ApproxEquals方法代码示例

        public void Vector3ApproxEquals()
        {
            Vector3 a = new Vector3(1f, 0f, 0f);
            Vector3 b = new Vector3(0f, 0f, 0f);

            Assert.IsFalse(a.ApproxEquals(b, 0.9f), "ApproxEquals failed (1)");
            Assert.IsTrue(a.ApproxEquals(b, 1.0f), "ApproxEquals failed (2)");

            a = new Vector3(-1f, 0f, 0f);
            b = new Vector3(1f, 0f, 0f);

            Assert.IsFalse(a.ApproxEquals(b, 1.9f), "ApproxEquals failed (3)");
            Assert.IsTrue(a.ApproxEquals(b, 2.0f), "ApproxEquals failed (4)");

            a = new Vector3(0f, -1f, 0f);
            b = new Vector3(0f, -1.1f, 0f);

            Assert.IsFalse(a.ApproxEquals(b, 0.09f), "ApproxEquals failed (5)");
            Assert.IsTrue(a.ApproxEquals(b, 0.11f), "ApproxEquals failed (6)");

            a = new Vector3(0f, 0f, 0.00001f);
            b = new Vector3(0f, 0f, 0f);

            Assert.IsFalse(b.ApproxEquals(a, Single.Epsilon), "ApproxEquals failed (6)");
            Assert.IsTrue(b.ApproxEquals(a, 0.0001f), "ApproxEquals failed (7)");
        }

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

//.........这里部分代码省略.........
            PreferredAxisOfMotion *= Quaternion.Add(rotq, m_referenceFrame);

            //Multiply it so that it scales linearly
            //deflection = PreferredAxisOfMotion;

            //deflection = ((PreferredAxisOfMotion * m_angularDeflectionEfficiency) / (m_angularDeflectionTimescale / pTimestep));

            #endregion
            
            #region Banking

            if (m_bankingEfficiency != 0)
            {
                Vector3 angularMotorVelocity = new Vector3 ();
                if (m_angularMotorApply > 0)
                {
                    // ramp up to new value
                    //   current velocity  +=                         error                       /    (time to get there / step interval)
                    //                               requested speed            -  last motor speed
                    angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
                    angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
                    angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);


                    // velocity may still be acheived.

                    Vector3 dir = Vector3.One * rotq;
                    float mult = /*dir.X > 0 ?*/ -1f /*: 1*/;
                    mult *= m_bankingMix;//Changes which way it banks in and out of turns

                    //Use the square of the efficiency, as it looks much more how SL banking works
                    float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
                    if (m_bankingEfficiency < 0)
                        effSquared *= -1;//Keep the negative!

                    banking.Z += (effSquared * (mult)) * (angularMotorVelocity.X);
                    m_angularMotorVelocity.X *= 1 - m_bankingEfficiency;
                    if (Math.Abs(m_lastAngularVelocity.Z) > m_bankingMix)
                    {
                        Vector3 bankingRot = new Vector3 (m_lastAngularVelocity.Z * (effSquared * 10 * (m_bankingMix * (-1))), 0, 0);
                        bankingRot *= rotq;
                        banking += bankingRot;
                        //m_angularMotorDirection.Y = m_lastAngularVelocity.Z * (m_bankingEfficiency);
                        //m_linearMotorDirectionLASTSET.Y = m_lastAngularVelocity.Z * (m_bankingEfficiency * 100);
                    }
                }
            }

            #endregion

            // Sum velocities
            m_lastAngularVelocity = m_angularMotorVelocity + vertattr + deflection + banking;

            if (!m_lastAngularVelocity.ApproxEquals (Vector3.Zero, 0.01f))
            {
                if (!d.BodyIsEnabled (Body))
                    d.BodyEnable (Body);
            }
            else
            {
                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
            }

            #region Linear Motor Offset

            //Offset section
            if (m_linearMotorOffset != Vector3.Zero)
            {
                //Offset of linear velocity doesn't change the linear velocity,
                //   but causes a torque to be applied, for example...
                //
                //      IIIII     >>>   IIIII
                //      IIIII     >>>    IIIII
                //      IIIII     >>>     IIIII
                //          ^
                //          |  Applying a force at the arrow will cause the object to move forward, but also rotate
                //
                //
                // The torque created is the linear velocity crossed with the offset

                //Note: we use the motor, otherwise you will just spin around and we divide by 10 since otherwise we go crazy
                Vector3 torqueFromOffset = (m_linearMotorDirectionLASTSET % m_linearMotorOffset) / 10;
                d.BodyAddTorque (Body, torqueFromOffset.X, torqueFromOffset.Y, torqueFromOffset.Z);
            }

            #endregion

            /*if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
            {
                m_lastAngularVelocity.X = 0;
                m_lastAngularVelocity.Y = 0;
            }*/

            // apply friction
            Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
            m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;

            // Apply to the body
            d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
        }

        public void TestLinkDelink2groups4SceneObjects()
        {
            TestHelpers.InMethod();
            
            bool debugtest = false;

            Scene scene = SceneHelpers.SetupScene();
            SceneObjectPart part1 = SceneHelpers.AddSceneObject(scene);
            SceneObjectGroup grp1 = part1.ParentGroup;
            SceneObjectPart part2 = SceneHelpers.AddSceneObject(scene);
            SceneObjectGroup grp2 = part2.ParentGroup;
            SceneObjectPart part3 = SceneHelpers.AddSceneObject(scene);
            SceneObjectGroup grp3 = part3.ParentGroup;
            SceneObjectPart part4 = SceneHelpers.AddSceneObject(scene);
            SceneObjectGroup grp4 = part4.ParentGroup;

            grp1.AbsolutePosition = new Vector3(10, 10, 10);
            grp2.AbsolutePosition = Vector3.Zero;
            grp3.AbsolutePosition = new Vector3(20, 20, 20);
            grp4.AbsolutePosition = new Vector3(40, 40, 40);

            // <90,0,0>
//            grp1.UpdateGroupRotationR(Quaternion.CreateFromEulers(90 * Utils.DEG_TO_RAD, 0, 0));

            // <180,0,0>
            grp2.UpdateGroupRotationR(Quaternion.CreateFromEulers(180 * Utils.DEG_TO_RAD, 0, 0));

            // <270,0,0>
//            grp3.UpdateGroupRotationR(Quaternion.CreateFromEulers(270 * Utils.DEG_TO_RAD, 0, 0));

            // <0,90,0>
            grp4.UpdateGroupRotationR(Quaternion.CreateFromEulers(0, 90 * Utils.DEG_TO_RAD, 0));

            // Required for linking
            grp1.RootPart.ClearUpdateSchedule();
            grp2.RootPart.ClearUpdateSchedule();
            grp3.RootPart.ClearUpdateSchedule();
            grp4.RootPart.ClearUpdateSchedule();

            // Link grp2 to grp1.   part2 becomes child prim to grp1. grp2 is eliminated.
            grp1.LinkToGroup(grp2);

            // Link grp4 to grp3.
            grp3.LinkToGroup(grp4);
            
            // At this point we should have 4 parts total in two groups.
            Assert.That(grp1.Parts.Length == 2, "Group1 children count should be 2");
            Assert.That(grp2.IsDeleted, "Group 2 was not registered as deleted after link.");
            Assert.That(grp2.Parts.Length, Is.EqualTo(0), "Group 2 still contained parts after delink.");
            Assert.That(grp3.Parts.Length == 2, "Group3 children count should be 2");
            Assert.That(grp4.IsDeleted, "Group 4 was not registered as deleted after link.");
            Assert.That(grp4.Parts.Length, Is.EqualTo(0), "Group 4 still contained parts after delink.");
            
            if (debugtest)
            {
                m_log.Debug("--------After Link-------");
                m_log.Debug("Group1: parts:" + grp1.Parts.Length);
                m_log.Debug("Group1: Pos:"+grp1.AbsolutePosition+", Rot:"+grp1.GroupRotation);
                m_log.Debug("Group1: Prim1: OffsetPosition:" + part1.OffsetPosition + ", OffsetRotation:" + part1.RotationOffset);
                m_log.Debug("Group1: Prim2: OffsetPosition:"+part2.OffsetPosition+", OffsetRotation:"+ part2.RotationOffset);

                m_log.Debug("Group3: parts:" + grp3.Parts.Length);
                m_log.Debug("Group3: Pos:"+grp3.AbsolutePosition+", Rot:"+grp3.GroupRotation);
                m_log.Debug("Group3: Prim1: OffsetPosition:"+part3.OffsetPosition+", OffsetRotation:"+part3.RotationOffset);
                m_log.Debug("Group3: Prim2: OffsetPosition:"+part4.OffsetPosition+", OffsetRotation:"+part4.RotationOffset);
            }

            // Required for linking
            grp1.RootPart.ClearUpdateSchedule();
            grp3.RootPart.ClearUpdateSchedule();

            // root part should have no offset position or rotation
            Assert.That(part1.OffsetPosition == Vector3.Zero && part1.RotationOffset == Quaternion.Identity,
                "root part should have no offset position or rotation (again)");

            // offset position should be root part position - part2.absolute position.
            Assert.That(part2.OffsetPosition == new Vector3(-10, -10, -10),
                "offset position should be root part position - part2.absolute position (again)");

            float roll = 0;
            float pitch = 0;
            float yaw = 0;

            // There's a euler anomoly at 180, 0, 0 so expect 180 to turn into -180.
            part1.RotationOffset.GetEulerAngles(out roll, out pitch, out yaw);
            Vector3 rotEuler1 = new Vector3(roll * Utils.RAD_TO_DEG, pitch * Utils.RAD_TO_DEG, yaw * Utils.RAD_TO_DEG);

            if (debugtest)
                m_log.Debug(rotEuler1);

            part2.RotationOffset.GetEulerAngles(out roll, out pitch, out yaw);
            Vector3 rotEuler2 = new Vector3(roll * Utils.RAD_TO_DEG, pitch * Utils.RAD_TO_DEG, yaw * Utils.RAD_TO_DEG);

            if (debugtest)
                m_log.Debug(rotEuler2);

            Assert.That(rotEuler2.ApproxEquals(new Vector3(-180, 0, 0), 0.001f) || rotEuler2.ApproxEquals(new Vector3(180, 0, 0), 0.001f),
                "Not sure what this assertion is all about...");

            // Now we're linking the first group to the third group.  This will make the first group child parts of the third one.
//.........这里部分代码省略.........

        public bool GetNextPosition(Vector3 currentPosition, float closeToRange, int secondsBeforeForcedTeleport,
                                    out Vector3 position, out TravelMode state, out bool needsToTeleportToPosition)
        {
            const bool found = false;

            lock (m_lock)
            {
                findNewTarget:
                position = Vector3.Zero;
                state = TravelMode.None;
                needsToTeleportToPosition = false;

                if ((m_listOfPositions.Count - m_currentpos) > 0)
                {
                    position = m_listOfPositions[m_currentpos];
                    state = m_listOfStates[m_currentpos];
                    if (state != TravelMode.Wait && state != TravelMode.TriggerHereEvent && position.ApproxEquals(currentPosition, closeToRange))
                    {
                        //Its close to a position, go look for the next pos
                        m_currentpos++;
                        m_lastChangedPosition = DateTime.MinValue;
                        goto findNewTarget;
                    }

                    if (state == TravelMode.TriggerHereEvent)
                    {
                        m_currentpos++; //Clear for next time, as we only fire this one time
                        m_lastChangedPosition = DateTime.MinValue;
                    }
                    else if (state == TravelMode.Wait)
                    {
                        if (m_waitingSince == DateTime.MinValue)
                            m_waitingSince = DateTime.Now;
                        else
                        {
                            if ((DateTime.Now - m_waitingSince).Seconds > position.X)
                            {
                                m_waitingSince = DateTime.MinValue;
                                m_currentpos++;
                                m_lastChangedPosition = DateTime.MinValue;
                                goto findNewTarget;
                            }
                        }
                    }
                    else
                    {
                        m_lastChangedPosition = DateTime.Now;
                        if ((DateTime.Now - m_lastChangedPosition).Seconds > secondsBeforeForcedTeleport)
                            needsToTeleportToPosition = true;
                    }

                    return true;
                }

                if (m_listOfPositions.Count == 0)          
                    return false;

                if (FollowIndefinitely)
                {
                    m_currentpos = 0; //Reset the position to the beginning if we have run out of positions
                    goto findNewTarget;
                }
            }

            return found;
        }

//.........这里部分代码省略.........
                        // If we're using the PID controller, then we have no gravity
                        //fz = (-1 * _parent_scene.gravityz) * m_mass;     //KF: ?? Prims have no global gravity,so simply...
                        fz = 0f;

                        //  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 ((m_PIDTau < 1) && (m_PIDTau != 0))
                        {
                            //PID_G = PID_G / m_PIDTau;
                            m_PIDTau = 1;
                        }

                        if ((PID_G - m_PIDTau) <= 0)
                        {
                            PID_G = m_PIDTau + 1;
                        }
                        //PidStatus = true;

                        // PhysicsVector vec = new PhysicsVector();

                        d.Vector3 pos = d.BodyGetPosition(Body);
                        _target_velocity =
                            new Vector3(
                                (float)(m_PIDTarget.X - pos.X) * ((/*PID_G - */m_PIDTau) * timestep),
                                (float)(m_PIDTarget.Y - pos.Y) * ((/*PID_G - */m_PIDTau) * timestep),
                                (float)(m_PIDTarget.Z - pos.Z) * ((/*PID_G - */m_PIDTau) * timestep)
                                );

                        //  if velocity is zero, use position control; otherwise, velocity control

                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
                        {
                            //  keep track of where we stopped.  No more slippin' & slidin'

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

                            //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
                            //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
                            //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
                            d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
                            if (!m_angularlock.ApproxEquals(Vector3.One, 0.003f) &&
                                Amotor != IntPtr.Zero)
                            {
                                
                            }
                            d.BodySetLinearVel(Body, 0, 0, 0);
                            d.BodyAddForce(Body, 0, 0, fz);
                            return;
                        }
                        else
                        {
                            _zeroFlag = false;

                            // We're flying and colliding with something
                            fx = (float)((_target_velocity.X) - vel.X) * (PID_D);
                            fy = (float)((_target_velocity.Y) - vel.Y) * (PID_D);

                            // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;

                            fz = (float)(fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass));

        /// <summary>
        /// Thread to update listener position and generally keep
        /// FMOD up to date.
        /// </summary>
        private void ListenerUpdate()
        {
            // Notice changes in position or direction.
            Vector3 lastpos = new Vector3(0.0f, 0.0f, 0.0f);
            float lastface = 0.0f;

            while (true)
            {
                // Two updates per second.
                Thread.Sleep(500);

                if (system == null) continue;

                AgentManager my = Instance.Client.Self;
                Vector3 newPosition = new Vector3(my.SimPosition);
                float newFace = my.SimRotation.W;

                // If we are standing still, nothing to update now, but
                // FMOD needs a 'tick' anyway for callbacks, etc.  In looping
                // 'game' programs, the loop is the 'tick'.   Since Radegast
                // uses events and has no loop, we use this position update
                // thread to drive the FMOD tick.  Have to move more than
                // 500mm or turn more than 10 desgrees to bother with.
                //
                if (newPosition.ApproxEquals(lastpos, 0.5f) &&
                    Math.Abs(newFace - lastface) < 0.2)
                {
                    invoke(new SoundDelegate(delegate
                    {
                        FMODExec(system.update());
                    }));
                    continue;
                }

                // We have moved or turned.  Remember new position.
                lastpos = newPosition;
                lastface = newFace;

                // Convert coordinate spaces.
                FMOD.VECTOR listenerpos = FromOMVSpace(newPosition);

                // Get azimuth from the facing Quaternion.  Note we assume the
                // avatar is standing upright.  Avatars in unusual positions
                // hear things from unpredictable directions.
                // By definition, facing.W = Cos( angle/2 )
                // With angle=0 meaning East.
                double angle = 2.0 * Math.Acos(newFace);

                // Construct facing unit vector in FMOD coordinates.
                // Z is East, X is South, Y is up.
                FMOD.VECTOR forward = new FMOD.VECTOR();
                forward.x = (float)Math.Sin(angle); // South
                forward.y = 0.0f;
                forward.z = (float)Math.Cos(angle); // East

                //Logger.Log(
                //    String.Format(
                //        "Standing at <{0:0.0},{1:0.0},{2:0.0}> facing {3:d}",
                //            listenerpos.x,
                //            listenerpos.y,
                //            listenerpos.z,
                //            (int)(angle * 180.0 / 3.141592)),
                //    Helpers.LogLevel.Debug);

                // Tell FMOD the new orientation.
                invoke(new SoundDelegate(delegate
                {
                    FMODExec(system.set3DListenerAttributes(
                        0,
                        ref listenerpos,    // Position
                        ref ZeroVector,        // Velocity
                        ref forward,        // Facing direction
                        ref UpVector));    // Top of head

                    FMODExec(system.update());
                }));
            }
        }

        internal void Move(float timestep)
        {
            //TODO:
            float fx = 0;
            float fy = 0;
            float fz = 0;

            if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero && !m_isSelected)
            {
                float m_mass = CalculateMass();

                fz = 0f;
                //m_log.Info(m_collisionFlags.ToString());

                if (m_buoyancy != 0)
                {
                    if (m_buoyancy > 0)
                    {
                        fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass) * 0.035f;

                        //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
                        //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + "  Pos: " + Position.ToString());
                    }
                    else
                    {
                        fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass) * 0.035f);
                    }
                }

                if (m_usePID)
                {
                    PID_D = 61f;
                    PID_G = 65f;
                    //if (!d.BodyIsEnabled(Body))
                    //d.BodySetForce(Body, 0f, 0f, 0f);
                    // If we're using the PID controller, then we have no gravity
                    fz = ((-1 * _parent_scene.gravityz) * m_mass) * 1.025f;

                    //  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 ((m_PIDTau < 1) && (m_PIDTau != 0))
                    {
                        //PID_G = PID_G / m_PIDTau;
                        m_PIDTau = 1;
                    }

                    if ((PID_G - m_PIDTau) <= 0)
                    {
                        PID_G = m_PIDTau + 1;
                    }

                    // TODO: NEED btVector3 for Linear Velocity
                    // NEED btVector3 for Position

                    Vector3 pos = _position; //TODO: Insert values gotten from bullet
                    Vector3 vel = _velocity;

                    _target_velocity =
                        new Vector3(
                            (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
                            (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
                            (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
                            );

                    if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
                    {

                        /* TODO: Do Bullet equiv
                         * 
                        d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
                        d.BodySetLinearVel(Body, 0, 0, 0);
                        d.BodyAddForce(Body, 0, 0, fz);
                        return;
                        */
                    }
                    else
                    {
                        _zeroFlag = false;

                        fx = ((_target_velocity.X) - vel.X) * (PID_D);
                        fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);

                    }

                }

                if (m_useHoverPID && !m_usePID)
                {
                    // If we're using the PID controller, then we have no gravity
                    fz = (-1 * _parent_scene.gravityz) * m_mass;

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

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

        // The physics engine says that properties have updated. Update same and inform
        // the world that things have changed.
        public void UpdateProperties(EntityProperties entprop)
        {
            bool changed = false;

            #region Updating Position

            if (entprop.ID != 0)
            {
                // we assign to the local variables so the normal set action does not happen
                if (_position != entprop.Position)
                {
                    _position = entprop.Position;
                    changed = true;
                }
                if (_orientation != entprop.Rotation)
                {
                    _orientation = entprop.Rotation;
                    changed = true;
                }
                if (_velocity != entprop.Velocity)
                {
                    changed = true;
                    _velocity = entprop.Velocity;
                }
                if (_acceleration != entprop.Acceleration)
                {
                    _acceleration = entprop.Acceleration;
                    changed = true;
                }
                if (_rotationalVelocity != entprop.RotationalVelocity)
                {
                    changed = true;
                    _rotationalVelocity = entprop.RotationalVelocity;
                }
                if (changed)
                    TriggerMovementUpdate();
            }

            #endregion

            #region Jump code

            if (_preJumping && Util.EnvironmentTickCountSubtract(_preJumpTime) > _scene.PreJumpTime)
            {
                //Time to jump
                _jumping = true;
                _preJumping = false;
                Velocity += _preJumpForce;
                _targetVelocityIsDecaying = false;
                TriggerMovementUpdate();
            }
            if (_jumping && Util.EnvironmentTickCountSubtract(_preJumpTime) > _scene.PreJumpTime + 2000)
            {
                _jumping = false;
                _targetVelocity = Vector3.Zero;
                TriggerMovementUpdate();
            }
            else if (_jumping && Util.EnvironmentTickCountSubtract(_preJumpTime) > _scene.PreJumpTime + 750)
            {
                _targetVelocityIsDecaying = false;
                TriggerMovementUpdate();
            }
            else if (_jumping && Util.EnvironmentTickCountSubtract(_preJumpTime) > _scene.PreJumpTime + 500)
            {
                _targetVelocityIsDecaying = false;
                Velocity -= _preJumpForce/100; //Cut down on going up
                TriggerMovementUpdate();
            }

            #endregion

            #region Decaying velocity

            if (_targetVelocityIsDecaying)
            {
                _targetVelocity *= _scene.DelayingVelocityMultiplier;
                if (_targetVelocity.ApproxEquals(Vector3.Zero, 0.1f) || _velocity == Vector3.Zero)
                    _targetVelocity = Vector3.Zero;
            }
            if (_targetVelocity != Vector3.Zero)
                Velocity = new Vector3(
                    _targetVelocity.X == 0
                        ? Velocity.X
                        : (_targetVelocity.X*0.25f) + (Velocity.X*0.75f),
                    _targetVelocity.Y == 0
                        ? Velocity.Y
                        : (_targetVelocity.Y*0.25f) + (Velocity.Y*0.75f),
                    _targetVelocity.Z == 0
                        ? Velocity.Z
                        : (_targetVelocity.Z*0.25f) + (Velocity.Z*0.75f));
            else if (Velocity.X != 0 || Velocity.Y != 0 || Velocity.Z > 0)
                Velocity *= _scene.DelayingVelocityMultiplier;
            if (Velocity.ApproxEquals(Vector3.Zero, 0.3f))
                Velocity = Vector3.Zero;

            #endregion
        }

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

            #endregion

            #region Apply the force

            if (IsFinite(vec))
            {
                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;

                    //ODE autodisables not moving prims, accept it and reenable when we need to
                    if (!d.BodyIsEnabled(Body))
                        d.BodyEnable(Body);

                    if (vec == Vector3.Zero) //if we arn't moving, STOP
                    {
                        if (m_lastForceApplied != -1)
                        {
                            m_lastForceApplied = -1;
                            d.BodySetLinearVel(Body, vec.X, vec.Y, vec.Z);
                        }
                    }
                    else
                    {
                        if (m_lastForceApplied < 5)
                            vec *= m_lastForceApplied / 5;
                        doForce(vec);
                        m_lastForceApplied++;
                    }

//                    if (!_zeroFlag && (!flying || m_iscolliding))
//                        AlignAvatarTiltWithCurrentDirectionOfMovement (vec, gravForce);

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

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

                    //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 DON'T it forces tons of updates

                    _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
            {
                MainConsole.Instance.Warn("[PHYSICS]: Got a NaN force vector in Move()");
                MainConsole.Instance.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);
                    Shell = IntPtr.Zero;
                }
            }

            #endregion
        }

        public void Move(float timestep)
        {
            float fx = 0;
            float fy = 0;
            float fz = 0;

            if (IsPhysical && Body != IntPtr.Zero && !m_isSelected)
            {
                //float PID_P = 900.0f;

                float m_mass = CalculateMass();

                fz = 0f;
                    //m_log.Info(m_collisionFlags.ToString());

                if (m_buoyancy != 0)
                {
                    if (m_buoyancy > 0)
                    {
                         fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass);

                        //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
                         //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + "  Pos: " + Position.ToString());
                    }
                    else
                    {
                        fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass));
                    }
                }

                if (m_usePID)
                {
                    // If we're using the PID controller, then we have no gravity
                    fz = (-1 * _parent_scene.gravityz) * m_mass;

                    //  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 ((m_PIDTau < 1))
                    {
                        PID_G = PID_G / m_PIDTau;
                    }

                    if ((PID_G - m_PIDTau) <= 0)
                    {
                        PID_G = m_PIDTau + 1;
                    }
                    //PidStatus = true;

                    // PhysicsVector vec = new PhysicsVector();
                    d.Vector3 vel = d.BodyGetLinearVel(Body);

                    d.Vector3 pos = d.BodyGetPosition(Body);
                    _target_velocity =
                        new Vector3(
                            (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
                            (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
                            (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
                            );

                    //  if velocity is zero, use position control; otherwise, velocity control

                    if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
                    {
                        //  keep track of where we stopped.  No more slippin' & slidin'

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

                        //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
                        //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
                        //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
                        d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
                        d.BodySetLinearVel(Body, 0, 0, 0);
                        d.BodyAddForce(Body, 0, 0, fz);
                        return;
                    }
                    else
                    {
                        _zeroFlag = false;

                        // We're flying and colliding with something
                        fx = ((_target_velocity.X) - vel.X) * (PID_D);
                        fy = ((_target_velocity.Y) - vel.Y) * (PID_D);

                        // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;

                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
                    }
                }

                fx *= m_mass;
                fy *= m_mass;
                //fz *= m_mass;

                fx += m_force.X;
//.........这里部分代码省略.........

        } // end MoveLinear()

        private void MoveAngular (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
        {
            d.Vector3 angularVelocity = d.BodyGetAngularVel (Body);
            d.Quaternion rot = d.BodyGetQuaternion (Body);
            Quaternion rotq = new Quaternion (rot.X, rot.Y, rot.Z, rot.W);
            //         Vector3 angularVelocity = Vector3.Zero;
            
            /*if ((m_flags & VehicleFlag.MOUSELOOK_STEER) == VehicleFlag.MOUSELOOK_STEER)
            {
                if (m_userLookAt != Quaternion.Identity)
                {
                    Quaternion camrot = Quaternion.Subtract (m_userLookAt, rotq);
                    camrot.Normalize ();
                    m_angularMotorVelocity += Vector3.One * camrot;
                    Console.WriteLine (Vector3.One * camrot);
                }
            }*/
            if (m_angularMotorApply > 90)
            {
                // ramp up to new value
                //   current velocity  +=                         error                       /    (time to get there / step interval)
                //                               requested speed            -  last motor speed
                m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / (pTimestep / 2));
                m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / (pTimestep / 2));
                m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / (pTimestep / 2));
                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
                // velocity may still be acheived.
                m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
            }
            else if(m_angularMotorVelocity != Vector3.Zero)
            {
                // no motor recently applied, keep the body velocity
                /*        m_angularMotorVelocity.X = angularVelocity.X;
                        m_angularMotorVelocity.Y = angularVelocity.Y;
                        m_angularMotorVelocity.Z = angularVelocity.Z; */

                // and decay the velocity
                m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / (pTimestep * 3));
                if(m_angularMotorVelocity.ApproxEquals(Vector3.Zero, 0.1f))
                    m_angularMotorVelocity = Vector3.Zero;
            } // end motor section
            if(m_angularMotorApply > 0)
                m_angularMotorApply--;

            // Vertical attractor section
            Vector3 vertattr = Vector3.Zero;
            Vector3 deflection = Vector3.Zero;
            Vector3 banking = Vector3.Zero;

            if(m_verticalAttractionTimescale < 300 && (m_lastAngularVelocity != Vector3.Zero || m_angularMotorApply > 90))
            {
                float VAservo = 0;
                if(Type == Vehicle.TYPE_BOAT)
                {
                    VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
                    VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
                }
                else
                {
                    if(parent.LinkSetIsColliding)
                        VAservo = 0.05f / (m_verticalAttractionTimescale * pTimestep);
                    else
                        VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
                    VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
                }
                // get present body rotation
                // make a vector pointing up
                Vector3 verterr = Vector3.Zero;
                verterr.Z = 1.0f;
                // rotate it to Body Angle
                verterr = verterr * rotq;
                // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
                // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
                // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
                if(verterr.Z < 0.0f)
                {
                    verterr.X = 2.0f - verterr.X;
                    verterr.Y = 2.0f - verterr.Y;
                }
                // Error is 0 (no error) to +/- 2 (max error)
                // scale it by VAservo
                verterr = verterr * VAservo;
                //if (frcount == 0) Console.WriteLine("VAerr=" + verterr);

                // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
                // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
                vertattr.X = verterr.Y;
                vertattr.Y = -verterr.X;
                vertattr.Z = 0f;

                // scaling appears better usingsquare-law
                float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
                vertattr.X += bounce * angularVelocity.X;
                vertattr.Y += bounce * angularVelocity.Y;
            } // else vertical attractor is off

            #region Deflection

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

        public void Move(float timestep)
            {
            if (m_frozen)
                return;
            float fx = 0;
            float fy = 0;
            float fz = 0;


            if (m_isphysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim)        // KF: Only move root prims.
                {
                if (m_vehicle.Type != Vehicle.TYPE_NONE)
                    {
                    // 'VEHICLES' are dealt with in ODEDynamics.cs
                    m_vehicle.Step(Body, timestep, _parent_scene, this);
                    d.Vector3 vel = d.BodyGetLinearVel(Body);
                    _velocity = new Vector3((float)vel.X, (float)vel.Y, (float)vel.Z);
                    d.Vector3 pos = d.GeomGetPosition(prim_geom);
                    _position = new Vector3((float)pos.X, (float)pos.Y, (float)pos.Z);
                    _zeroFlag = false;
                    }
                else
                    {
                    float m_mass = _mass;
                    d.Vector3 dcpos = d.BodyGetPosition(Body);
                    d.Vector3 vel = d.BodyGetLinearVel(Body);
                    d.Vector3 angvel = d.BodyGetAngularVel(Body);

                    //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
                    // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
                    // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
                    // gravityz multiplier = 1 - m_buoyancy
                    if (!_parent_scene.UsePointGravity)
                        {
                        if (!testRealGravity)
                            {
                            fx = _parent_scene.gravityx * (1.0f - m_buoyancy);
                            fy = _parent_scene.gravityy * (1.0f - m_buoyancy);
                            fz = _parent_scene.gravityz * (1.0f - m_buoyancy);
                            }
                        else
                            {
                            fx = _parent_scene.gravityx * -1 * (1.0f - m_buoyancy);
                            fy = _parent_scene.gravityy * -1 * (1.0f - m_buoyancy);
                            fz = _parent_scene.gravityz * -1 * (1.0f - m_buoyancy);
                            }
                        }
                    else
                        {
                        //Set up point gravity for this object
                        Vector3 cog = _parent_scene.PointOfGravity;
                        if (cog.X != 0)
                            fx = (cog.X - dcpos.X);
                        if (cog.Y != 0)
                            fy = (cog.Y - dcpos.Y);
                        if (cog.Z != 0)
                            fz = (cog.Z - dcpos.Z);
                        }


                    #region PID
                    if (m_usePID)
                        {
                        //Console.WriteLine("PID " +  m_primName);
                        // KF - this is for object move? eg. llSetPos() ?
                        //if (!d.BodyIsEnabled(Body))
                        //d.BodySetForce(Body, 0f, 0f, 0f);
                        // If we're using the PID controller, then we have no gravity
                        //fz = (-1 * _parent_scene.gravityz) * m_mass;     //KF: ?? Prims have no global gravity,so simply...
                        fz = 0f;

                        //  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 ((m_PIDTau < 1) && (m_PIDTau != 0))
                            {
                            //PID_G = PID_G / m_PIDTau;
                            m_PIDTau = 1;
                            }

                        if ((PID_G - m_PIDTau) <= 0)
                            {
                            PID_G = m_PIDTau + 1;
                            }
                        //PidStatus = true;

                        // PhysicsVector vec = new PhysicsVector();

                        _target_velocity =
                            new Vector3(
                                (float)(m_PIDTarget.X - dcpos.X) * ((/*PID_G - */m_PIDTau) * timestep),
                                (float)(m_PIDTarget.Y - dcpos.Y) * ((/*PID_G - */m_PIDTau) * timestep),
                                (float)(m_PIDTarget.Z - dcpos.Z) * ((/*PID_G - */m_PIDTau) * timestep)
                                );

                        //  if velocity is zero, use position control; otherwise, velocity control

                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.05f))
//.........这里部分代码省略.........

 public virtual bool ErrorIsZero(Vector3 err)
 {
     return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
 }

        internal void ProcessGeomCreationAsTriMesh(Vector3 positionOffset, Quaternion orientation)
        {
            // Don't need to re-enable body..   it's done in SetMesh
            float meshlod = _parent_scene.meshSculptLOD;

            if (IsPhysical)
                meshlod = _parent_scene.MeshSculptphysicalLOD;

            IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
            if (!positionOffset.ApproxEquals(Vector3.Zero, 0.001f) || orientation != Quaternion.Identity)
            {

                float[] xyz = new float[3];
                xyz[0] = positionOffset.X;
                xyz[1] = positionOffset.Y;
                xyz[2] = positionOffset.Z;

                Matrix4 m4 = Matrix4.CreateFromQuaternion(orientation);

                float[,] matrix = new float[3, 3];

                matrix[0, 0] = m4.M11;
                matrix[0, 1] = m4.M12;
                matrix[0, 2] = m4.M13;
                matrix[1, 0] = m4.M21;
                matrix[1, 1] = m4.M22;
                matrix[1, 2] = m4.M23;
                matrix[2, 0] = m4.M31;
                matrix[2, 1] = m4.M32;
                matrix[2, 2] = m4.M33;


                mesh.TransformLinear(matrix, xyz);



            }

            _mesh = mesh;
        }