示例#1
0
        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)");
        }
示例#2
0
        /// <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

            #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 Check for underground

            if (!(Position.X < 0.25f || Position.Y < 0.25f ||
                Position.X > _parent_scene.Region.RegionSizeX - .25f ||
                Position.Y > _parent_scene.Region.RegionSizeY - .25f))
            {
                float groundHeight = _parent_scene.GetTerrainHeightAtXY (
                           tempPos.X + (tempPos.X == 0 ? tempPos.X : timeStep * 0.75f * vel.X),
                           tempPos.Y + (tempPos.Y == 0 ? tempPos.Y : timeStep * 0.75f * vel.Y));

                if ((tempPos.Z - AvatarHalfsize) < groundHeight)
                {
                    if (!flying)
                    {
                        //if (_target_velocity.Z < 0)
                        _target_velocity.Z = 0;
                        vec.Z = -vel.Z * PID_D * 2f + ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P * 100.0f);
                    }
                    else
                        vec.Z = ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P);
                }
                if (tempPos.Z - AvatarHalfsize - groundHeight < 0.12f)
                {
                    m_iscolliding = true;
                    m_iscollidingGround = true;
                    flying = false; // ground the avatar
                    ContactPoint point = new ContactPoint ();
                    point.PenetrationDepth = vel.Z;
                    point.Position = Position;
                    point.SurfaceNormal = Vector3.Zero;

                    //0 is the ground localID
                    this.AddCollisionEvent (0, point);
                }
                else
                    m_iscollidingGround = false;
            }
            else
                m_iscollidingGround = true;//Assume that they are crossing, and keep them from falling down

            #endregion

            #region Movement

            #region Jump code

            if (IsJumping && (IsColliding && m_preJumpCounter > _parent_scene.m_preJumpTime + 10) || m_preJumpCounter > 50)
            {
                m_isJumping = false;
                m_preJumpCounter = 0;
            }
            if (IsJumping)
                m_preJumpCounter++;

            if (m_ispreJumping && m_preJumpCounter == _parent_scene.m_preJumpTime)
            {
                m_ispreJumping = false;
                _target_velocity += m_preJumpForce * _parent_scene.m_preJumpForceMultiplier;
                m_preJumpCounter = 0;
                m_isJumping = true;
            }
            if (m_ispreJumping)
            {
                m_preJumpCounter++;
                TriggerMovementUpdate();
                return;
            }

            #endregion

            //  if velocity is zero, use position control; otherwise, velocity control
            if (_target_velocity == Vector3.Zero &&
                Math.Abs (vel.X) < 0.05 && Math.Abs (vel.Y) < 0.05 && Math.Abs (vel.Z) < 0.05 && (this.m_iscollidingGround || this.m_iscollidingObj || this.flying))
            //This is so that if we get moved by something else, it will update us in the client
            {
                m_isJumping = false;
                //  keep track of where we stopped.  No more slippin' & slidin'
                if (!_zeroFlag)
                {
                    _zeroFlag = true;
                    _zeroPosition = tempPos;
                }

                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
                    // if target vel is zero why was it here ?
                    vec.X = -vel.X * PID_D + (_zeroPosition.X - tempPos.X) * PID_P;
                    vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - tempPos.Y) * PID_P;
                }
            }
            else
            {
                m_pidControllerActive = true;
                _zeroFlag = false;

                if (m_iscolliding)
                {
                    if (!flying)
                    {
                        if (_target_velocity.Z != 0.0f)
                            vec.Z = (_target_velocity.Z * movementmult - vel.Z) * PID_D;// + (_zeroPosition.Z - tempPos.Z) * PID_P)) _zeropos maybe bad here
                        // We're standing or walking on something
                        vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 2;
                        vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 2;
                    }
                    else
                    {
                        // We're flying and colliding with something
                        vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.5f;
                        vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.5f;
                    }
                }
                else
                {
                    if (flying)
                    {
                        // we're flying
                        vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.75f;
                        vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.75f;
                    }
                    else
                    {
                        // we're not colliding and we're not flying so that means we're falling!
                        // m_iscolliding includes collisions with the ground.
                        vec.X = (_target_velocity.X - vel.X) * PID_D * 0.85f;
                        vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.85f;
                    }
                }
                if (flying)
                {
                    #region Av gravity

                    if (_parent_scene.AllowAvGravity &&
                        tempPos.Z > _parent_scene.AvGravityHeight) //Should be stop avies from flying upwards
                    {
                        //Decay going up 
                        if (_target_velocity.Z > 0)
                        {
                            //How much should we force them down?
                            float Multiplier = (_parent_scene.AllowAvsToEscapeGravity ? .03f : .1f);
                            //How much should we force them down?
                            float fudgeHeight = (_parent_scene.AllowAvsToEscapeGravity ? 80 : 30);
                            //We add the 30 so that gravity is resonably strong once they pass the min height
                            Multiplier *= tempPos.Z + fudgeHeight - _parent_scene.AvGravityHeight;

                            //Limit these so that things don't go wrong
                            if (Multiplier < 1)
                                Multiplier = 1;

                            float maxpower = (_parent_scene.AllowAvsToEscapeGravity ? 1.5f : 3f);

                            if (Multiplier > maxpower)
                                Multiplier = maxpower;

                            _target_velocity.Z /= Multiplier;
                            vel.Z /= Multiplier;
                        }
                    }

                    #endregion

                    vec.Z = (_target_velocity.Z * movementmult - vel.Z) * PID_D * 0.5f;
                    if (_parent_scene.AllowAvGravity && tempPos.Z > _parent_scene.AvGravityHeight)
                        //Add extra gravity
                        vec.Z += ((10 * _parent_scene.gravityz) * Mass);
                }
            }

            if (flying)
            {
                #region Auto Fly Height

                //Added for auto fly height. Kitto Flora
                //Changed to only check if the avatar is flying around,

                // Revolution: If the avatar is going down, they are trying to land (probably), so don't push them up to make it harder
                //   Only if they are moving around sideways do we need to push them up
                if (_target_velocity.X != 0 || _target_velocity.Y != 0)
                {
                    Vector3 forwardVel = new Vector3 (_target_velocity.X > 0 ? 2 : (_target_velocity.X < 0 ? -2 : 0),
                        _target_velocity.Y > 0 ? 2 : (_target_velocity.Y < 0 ? -2 : 0),
                        0);
                    float target_altitude = _parent_scene.GetTerrainHeightAtXY (tempPos.X, tempPos.Y) + MinimumGroundFlightOffset;

                    //We cheat a bit and do a bit lower than normal
                    if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude ||
                            (tempPos.Z - CAPSULE_LENGTH) < _parent_scene.GetTerrainHeightAtXY (tempPos.X + forwardVel.X, tempPos.Y + forwardVel.Y)
                            + MinimumGroundFlightOffset)
                        vec.Z += (target_altitude - tempPos.Z) * PID_P * 0.5f;
                }
                else
                {
                    //Straight up and down, only apply when they are very close to the ground
                    float target_altitude = _parent_scene.GetTerrainHeightAtXY (tempPos.X, tempPos.Y);

                    if ((tempPos.Z - CAPSULE_LENGTH + (MinimumGroundFlightOffset / 1.5)) < target_altitude + MinimumGroundFlightOffset)
                    {
                        if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude + 1)
                        {
                            vec.Z += ((target_altitude + 4) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P;
                        }
                        else
                            vec.Z += ((target_altitude + MinimumGroundFlightOffset) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P * 0.5f;
                    }
                }

                #endregion
            }

            #region Gravity

            if (!flying && _parent_scene.AllowAvGravity)
            {
                if (!_parent_scene.UsePointGravity)
                {
                    //Add normal gravity
                    vec.X += _parent_scene.gravityx * m_mass;
                    vec.Y += _parent_scene.gravityy * m_mass;
                    vec.Z += _parent_scene.gravityz * m_mass;
                }
                else
                {
                    Vector3 cog = _parent_scene.PointOfGravity;
                    if (cog.X != 0)
                        vec.X += (cog.X - tempPos.X) * m_mass;
                    if (cog.Y != 0)
                        vec.Y += (cog.Y - tempPos.Y) * m_mass;
                    if (cog.Z != 0)
                        vec.Z += (cog.Z - tempPos.Z) * m_mass;
                }
            }

            #endregion

            #region Under water physics

            if (_parent_scene.AllowUnderwaterPhysics && (float)tempPos.X < _parent_scene.Region.RegionSizeX &&
                (float)tempPos.Y < _parent_scene.Region.RegionSizeY)
            {
                //Position plus height to av's shoulder (aprox) is just above water
                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
        }
        } // 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 > 0)
            {
                // 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);
                m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
                m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);

                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
                // velocity may still be acheived.
            }
            else
            {
                // 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 * pTimestep));
            } // end motor section

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

            if (m_verticalAttractionTimescale < 300)
            {
                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
                // 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

            //        m_lastVertAttractor = vertattr;

            // Bank section tba
            
            #region Deflection

            //Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well
            Vector3 PreferredAxisOfMotion = new Vector3 ((10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale) * pTimestep), 0, 0);
            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);
        }
示例#4
0
        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.
            grp3.LinkToGroup(grp1);

            // Delink parts 2 and 3
            grp3.DelinkFromGroup(part2.LocalId);
            grp3.DelinkFromGroup(part3.LocalId);

            if (debugtest)
            {
                m_log.Debug("--------After De-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);
            }

            Assert.That(part2.AbsolutePosition == Vector3.Zero, "Badness 1");
            Assert.That(part4.OffsetPosition == new Vector3(20, 20, 20), "Badness 2");
            Quaternion compareQuaternion = new Quaternion(0, 0.7071068f, 0, 0.7071068f);
            Assert.That((part4.RotationOffset.X - compareQuaternion.X < 0.00003) 
                && (part4.RotationOffset.Y - compareQuaternion.Y < 0.00003) 
                && (part4.RotationOffset.Z - compareQuaternion.Z < 0.00003) 
                && (part4.RotationOffset.W - compareQuaternion.W < 0.00003),
                "Badness 3");
        }
        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;
        }
示例#6
0
        public void Move(float timestep)
        {
            if (m_frozen)
                return;
            float fx = 0;
            float fy = 0;
            float fz = 0;

                
            if (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.BodyGetPosition(Body);
                    _position = new Vector3((float)pos.X, (float)pos.Y, (float)pos.Z);
                    _zeroFlag = false;
                }
                else
                {
                    //Console.WriteLine("Move " +  m_primName);
                    //if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
                    // NON-'VEHICLES' are dealt with here
                    //float PID_P = 900.0f;

                    float m_mass = Mass;

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


                    //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) * m_mass;
                            fy = _parent_scene.gravityy * (1.0f - m_buoyancy) * m_mass;
                            fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
                        }
                        else
                        {
                            fx = _parent_scene.gravityx * -1 * (m_buoyancy);
                            fy = _parent_scene.gravityy * -1 * (m_buoyancy);
                            fz = _parent_scene.gravityz * -1 * (m_buoyancy);
                        }
                    }
                    else
                    {
                        //Set up point gravity for this object
                        Vector3 cog = _parent_scene.PointOfGravity;
                        if (cog.X != 0)
                            fx = (cog.X - Position.X) * m_mass;
                        if (cog.Y != 0)
                            fy = (cog.Y - Position.Y) * m_mass;
                        if (cog.Z != 0)
                            fz = (cog.Z - Position.Z) * m_mass;
                    }

                    d.Vector3 vel = d.BodyGetLinearVel(Body);
                    d.Vector3 force = d.BodyGetForce(Body);
                    d.Vector3 angvel = d.BodyGetAngularVel(Body);

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

                        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));
                        }
                    }        // end if (m_usePID)
                    #endregion
                    #region Hover PID
                    // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
                    if (m_useHoverPID && !m_usePID)
                    {
                        //Console.WriteLine("Hover " +  m_primName);

                        // If we're using the PID controller, then we have no gravity
                        fx = (-1 * _parent_scene.gravityx) * m_mass;
                        fy = (-1 * _parent_scene.gravityy) * m_mass;
                        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;
                        }


                        // Where are we, and where are we headed?
                        d.Vector3 pos = d.BodyGetPosition(Body);


                        //    Non-Vehicles have a limited set of Hover options.
                        // determine what our target height really is based on HoverType
                        switch (m_PIDHoverType)
                        {
                            case PIDHoverType.Ground:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY((float)pos.X, (float)pos.Y);
                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                break;
                            case PIDHoverType.GroundAndWater:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY((float)pos.X, (float)pos.Y);
                                m_waterHeight = _parent_scene.GetWaterLevel();
                                if (m_groundHeight > m_waterHeight)
                                {
                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                }
                                else
                                {
                                    m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
                                }
                                break;

                        }     // end switch (m_PIDHoverType)


                        _target_velocity =
                            new Vector3(0.0f, 0.0f,
                                (float)(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * 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

                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
                            d.BodyAddForce(Body, 0, 0, fz);
                            return;
                        }
                        else
                        {
                            _zeroFlag = false;

                            // We're flying and colliding with something
                            fz = (float)(fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass));
                        }
                    }
                    #endregion

                    fx *= m_mass;
                    fy *= m_mass;

                    fx += m_force.X;
                    fy += m_force.Y;
                    fz += m_force.Z;

                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
                    if (fx != 0 || fy != 0 || fz != 0)
                    {
                        // 35n times the mass per second applied maximum.
                        float nmax = 35f * m_mass;
                        float nmin = -35f * m_mass;


                        if (fx > nmax)
                            fx = nmax;
                        if (fx < nmin)
                            fx = nmin;
                        if (fy > nmax)
                            fy = nmax;
                        if (fy < nmin)
                            fy = nmin;

                        if (!m_angularlock.ApproxEquals(Vector3.One, 0.003f) &&
                                Amotor != IntPtr.Zero)
                        {
                            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0.0001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.0001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.0001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.0001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.0001f);
                        }

                        if (vel.Z < -30)
                        {
                            vel.Z = -30;
                        }

                        bool disabled = false;

                        if (_parent_scene.m_DisableSlowPrims)
                        {
                            if (((float)fz == (float)(_parent_scene.gravityz * m_mass)) &&
                                (vel.X < 0.01 || vel.Y < 0.01 || vel.Z < 0.0001) &&
                                m_forcelist.Count == 0)
                            {
                                if (Math.Abs(vel.X) < 0.0001 || Math.Abs(vel.Y) < 0.0001 || Math.Abs(vel.Z) < 0.0001)
                                {
                                    Vector3 angvelocity = new Vector3((float)angvel.X, (float)angvel.Y, (float)angvel.Z);
                                    if (Math.Abs(force.X) < 100 && Math.Abs(force.Y) < 100 && Math.Abs(force.Z) < 100 &&
                                        angvelocity.ApproxEquals(Vector3.Zero, 0.01f) &&
                                        vel.X != 0 && vel.Y != 0 && vel.Z != 0)
                                    {
                                        if (d.BodyIsEnabled(Body))
                                        {
                                            d.BodySetLinearVel(Body, 0, 0, 0);
                                            d.BodySetForce(Body, 0, 0, 0);
                                            d.BodyDisable(Body);
                                            disabled = true;
                                        }
                                    }
                                    else
                                    {
                                        if (!d.BodyIsEnabled(Body))
                                            d.BodyEnable(Body);
                                    }
                                }
                                else
                                {
                                    if (!d.BodyIsEnabled(Body))
                                    {
                                        d.BodyEnable(Body);
                                        fz = 100 * m_mass;
                                    }
                                }
                            }
                        }

                        if (!disabled)
                        {
                            if (!d.BodyIsEnabled(Body))
                            {
                                // A physical body at rest on a surface will auto-disable after a while,
                                // this appears to re-enable it incase the surface it is upon vanishes,
                                // and the body should fall again. 
                                d.BodySetLinearVel(Body, 0f, 0f, m_mass);
                                d.BodySetForce(Body, 0, 0, 100 * m_mass);
                                enableBodySoft();
                                vel = d.BodyGetLinearVel(Body);
                            }

                            float Drag = Mass / 2;

                            if (fx != 0)
                                fx -= Drag;
                            if (fy != 0)
                                fy -= Drag;
                            if (fz < 0)
                                fz += Drag;
                            else
                                fz -= Drag;

                            #region Force List

                            Vector3 iforce = Vector3.Zero;
                            int i = 0;
                            try
                            {
                                for (i = 0; i < m_forcelist.Count; i++)
                                {
                                    iforce = iforce + (m_forcelist[i] * 100);
                                }
                            }
                            catch (IndexOutOfRangeException)
                            {
                                m_forcelist = new List<Vector3>();
                                m_collisionscore = 0;
                                m_interpenetrationcount = 0;
                                m_taintforce = false;
                                return;
                            }
                            catch (ArgumentOutOfRangeException)
                            {
                                m_forcelist = new List<Vector3>();
                                m_collisionscore = 0;
                                m_interpenetrationcount = 0;
                                m_taintforce = false;
                                return;
                            }
                            fx += iforce.X;
                            fy += iforce.Y;
                            fz += iforce.Z;
                            m_forcelist.Clear();

                            #endregion

                            d.BodyAddForce(Body, fx, fy, fz);
                        }
                    }
                }
            }
            else
            {    // is not physical, or is not a body or is selected
              //  _zeroPosition = d.BodyGetPosition(Body);
                return;
//Console.WriteLine("Nothing " +  m_primName);
               
            }
        }
示例#7
0
        /// <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());
                }));
            }
        }
示例#8
0
        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

                    if ((m_PIDTau < 1))
                    {
                        PID_G = PID_G / m_PIDTau;
                    }

                    if ((PID_G - m_PIDTau) <= 0)
                    {
                        PID_G = m_PIDTau + 1;
                    }
                    Vector3 pos = Vector3.Zero; //TODO: Insert values gotten from bullet
                    Vector3 vel = Vector3.Zero;

                    // determine what our target height really is based on HoverType
                    switch (m_PIDHoverType)
                    {
                        case PIDHoverType.Absolute:
                            m_targetHoverHeight = m_PIDHoverHeight;
                            break;
                        case PIDHoverType.Ground:
                            m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
                            m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                            break;
                        case PIDHoverType.GroundAndWater:
                            m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
                            m_waterHeight = _parent_scene.GetWaterLevel();
                            if (m_groundHeight > m_waterHeight)
                            {
                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                            }
                            else
                            {
                                m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
                            }
                            break;
                        case PIDHoverType.Water:
                            m_waterHeight = _parent_scene.GetWaterLevel();
                            m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
                            break;
                    }


                    _target_velocity =
                        new Vector3(0.0f, 0.0f,
                            (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
                            );

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

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

                        /* TODO: Do Bullet Equiv
                        d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
                        d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
                        d.BodyAddForce(Body, 0, 0, fz);
                        */
                        if (Body != null && Body.Handle != IntPtr.Zero)
                        {
                            Body.setLinearVelocity(_parent_scene.VectorZero);
                            Body.clearForces();
                        }
                        return;
                    }
                    else
                    {
                        _zeroFlag = false;

                        // We're flying and colliding with something
                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
                    }
                }

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

                fx += m_force.X;
                fy += m_force.Y;
                fz += m_force.Z;

                //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
                if (fx != 0 || fy != 0 || fz != 0)
                {
                    /*
                     * TODO: Do Bullet Equiv
                    if (!d.BodyIsEnabled(Body))
                    {
                        d.BodySetLinearVel(Body, 0f, 0f, 0f);
                        d.BodySetForce(Body, 0, 0, 0);
                        enableBodySoft();
                    }
                    */
                    // 35x10 = 350n times the mass per second applied maximum.

                    float nmax = 35f * m_mass;
                    float nmin = -35f * m_mass;


                    if (fx > nmax)
                        fx = nmax;
                    if (fx < nmin)
                        fx = nmin;
                    if (fy > nmax)
                        fy = nmax;
                    if (fy < nmin)
                        fy = nmin;

                    // TODO: Do Bullet Equiv
                    // d.BodyAddForce(Body, fx, fy, fz);
                    if (Body != null && Body.Handle != IntPtr.Zero)
                    {
                        Body.activate(true);
                        if (tempAddForce != null && tempAddForce.Handle != IntPtr.Zero)
                            tempAddForce.Dispose();

                        tempAddForce = new btVector3(fx * 0.01f, fy * 0.01f, fz * 0.01f);
                        Body.applyCentralImpulse(tempAddForce);
                    }
                }
            }
            else
            {
                if (m_zeroPosition == null)
                    m_zeroPosition = Vector3.Zero;
                m_zeroPosition = _position;
                return;
            }
        }
示例#9
0
        // 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
        }
示例#10
0
        /// <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;

            Vector3 vec = Vector3.Zero;
            d.Vector3 vel = d.BodyGetLinearVel(Body);
            d.Vector3 tempPos;
            d.BodyCopyPosition(Body, out tempPos);

            #region Flight Ceiling

            // rex, added height check

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

            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 (_target_velocity.Z > 0.0f)
                    _target_velocity.Z = 0.0f;
            }

            // endrex

            #endregion

            #region NonFinite Pos

            Vector3 localPos = new Vector3(tempPos.X, tempPos.Y, tempPos.Z);

            if (!IsFinite(localPos))
            {
                MainConsole.Instance.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);
                    Shell = IntPtr.Zero;
                }
                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;
//            movementmult *= 1 / _parent_scene.TimeDilation;
            if (flying)
                movementmult *= _parent_scene.m_AvFlySpeed;

            #endregion

            #region Check for underground

            d.AABB aabb;
            d.GeomGetAABB(Shell, out aabb);
            float chrminZ = aabb.MinZ;

            Vector3 posch = localPos;

            float ftmp;

            if (flying)
            {
                ftmp = 0.75f*timeStep;
                posch.X += vel.X*ftmp;
                posch.Y += vel.Y*ftmp;
            }

            float groundHeight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);

            if (chrminZ < groundHeight)
            {
                float depth = groundHeight - chrminZ;

                if (_target_velocity.Z < 0)
                    _target_velocity.Z = 0;

                if (!flying)
                {
                    if (vel.Z < -10f)
                        vel.Z = -10f;
                    vec.Z = -vel.Z*PID_D*1.5f + depth*PID_P*50.0f;
                }
                else
                {
                    vec.Z = depth*PID_P*50.0f;
                }

                if (depth < 0.12f)
                {
                    m_iscolliding = true;
                    m_colliderfilter = 2;
                    m_iscollidingGround = true;

                    ContactPoint point = new ContactPoint
                                             {
                                                 Type = ActorTypes.Ground,
                                                 PenetrationDepth = depth,
                                                 Position = {X = localPos.X, Y = localPos.Y, Z = chrminZ},
                                                 SurfaceNormal = new Vector3(0, 0, -1f)
                                             };

                    //0 is the ground localID
                    AddCollisionEvent(0, point);
                    vec.Z *= 0.5f;
                }
                else
                    m_iscollidingGround = false;
            }
            else
                m_iscollidingGround = false;
/*
            if(Flying && _target_velocity == Vector3.Zero &&
                Math.Abs(vel.Z) < 0.1)
                notMoving = true;
*/

            #endregion

            #region Movement

            #region Jump code

            if (IsJumping)
            {
//                if ((IsColliding) && m_preJumpCounter > _parent_scene.m_preJumpTime || m_preJumpCounter > 150)
                if ((IsColliding) && m_preJumpCounter > _parent_scene.m_preJumpTime || m_preJumpCounter > 150)
                {
                    m_isJumping = false;
                    m_preJumpCounter = 0;
                    _target_velocity.Z = 0;
                }
                else
                    m_preJumpCounter++;
            }

            else if (m_ispreJumping)
            {
                if (m_preJumpCounter == _parent_scene.m_preJumpTime)
                {
                    m_ispreJumping = false;
                    _target_velocity.X = m_preJumpForce.X*_parent_scene.m_preJumpForceMultiplierX;
                    _target_velocity.Y = m_preJumpForce.Y*_parent_scene.m_preJumpForceMultiplierY;
                    _target_velocity.Z = m_preJumpForce.Z*_parent_scene.m_preJumpForceMultiplierZ;

                    m_preJumpCounter = 0;
                    m_isJumping = true;
                }
                else
                {
                    m_preJumpCounter++;
                    TriggerMovementUpdate();
                    return;
                }
            }


            //This is for jumping on prims, since otherwise, you don't get off the ground sometimes
//            if (m_iscolliding && m_isJumping && _target_velocity.Z < 1 && !Flying)
//                _target_velocity.Z += m_preJumpForce.Z * _parent_scene.m_preJumpForceMultiplier;

            #endregion

            Vector3 gravForce = new Vector3();

            //  if velocity is zero, use position control; otherwise, velocity control
            if (_target_velocity == Vector3.Zero &&
                Math.Abs(vel.X) < 0.1 && Math.Abs(vel.Y) < 0.1 && Math.Abs(vel.Z) < 0.1 &&
                (this.m_iscolliding || this.flying || (this._zeroFlag && _wasZeroFlagFlying == flying)))
                //This is so that if we get moved by something else, it will update us in the client
            {
                m_isJumping = false;
                //  keep track of where we stopped.  No more slippin' & slidin'
                if (!_zeroFlag)
                {
                    _zeroFlag = true;
                    _wasZeroFlagFlying = flying;
                    _zeroPosition = tempPos;
                }

                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
                    // if target vel is zero why was it here ?
                    vec.X = -vel.X*PID_D + (_zeroPosition.X - tempPos.X)*PID_P;
                    vec.Y = -vel.Y*PID_D + (_zeroPosition.Y - tempPos.Y)*PID_P;
//                    if (!realFlying)
//                        vec.Z +=  - vel.Z * PID_D * 5;
//                    else
                    if (flying)
                        vec.Z += -vel.Z*PID_D*0.5f + (_zeroPosition.Z - tempPos.Z)*PID_P;

//                    _parent_scene.CalculateGravity(m_mass, tempPos, true, 0.15f, ref gravForce);
//                    vec += gravForce;
                }
            }
            else
            {
                m_pidControllerActive = true;
                _zeroFlag = false;

                if (m_iscolliding)
                {
                    if (!flying) //If there is a ground collision, it sets flying to false, so check against real flying
                    {
                        // We're standing or walking on something
                        if (_target_velocity.X != 0.0f)
                            vec.X += (_target_velocity.X * movementmult - vel.X) * PID_D * 2;
                        if (_target_velocity.Y != 0.0f)
                            vec.Y += (_target_velocity.Y * movementmult - vel.Y) * PID_D * 2;
                        if (_target_velocity.Z != 0.0f)
                            vec.Z += (_target_velocity.Z * movementmult - vel.Z) * PID_D;
                        /*// We're standing or walking on something
                        vec.X += (_target_velocity.X*movementmult - vel.X)*PID_D*2;
                        vec.Y += (_target_velocity.Y*movementmult - vel.Y)*PID_D*2;
                        if (_target_velocity.Z > 0.0f)
                            vec.Z += (_target_velocity.Z*movementmult - vel.Z)*PID_D;
                                // + (_zeroPosition.Z - tempPos.Z) * PID_P)) _zeropos maybe bad here*/
                    }
                    else
                    {
                        // We're flying and colliding with something
                        vec.X += (_target_velocity.X*movementmult - vel.X)*PID_D*0.5f;
                        vec.Y += (_target_velocity.Y*movementmult - vel.Y)*PID_D*0.5f;
                        //if(_target_velocity.Z > 0)
                        vec.Z += (_target_velocity.Z*movementmult - vel.Z)*PID_D*0.5f;
                    }
                }
                else
                {
                    if (flying)
                    {
                        // we're flying
                        vec.X += (_target_velocity.X * movementmult - vel.X) * PID_D * 0.75f;
                        vec.Y += (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.75f;
                    }
                    else
                    {
                        // we're not colliding and we're not flying so that means we're falling!
                        // m_iscolliding includes collisions with the ground.
                        vec.X += (_target_velocity.X - vel.X) * PID_D * 0.85f;
                        vec.Y += (_target_velocity.Y - vel.Y) * PID_D * 0.85f;
                    }
                }


                if (flying)
                {
                    #region Av gravity

                    if (_parent_scene.AllowAvGravity &&
                        tempPos.Z > _parent_scene.AvGravityHeight) //Should be stop avies from flying upwards
                    {
                        //Decay going up 
                        if (_target_velocity.Z > 0)
                        {
                            //How much should we force them down?
                            float Multiplier = (_parent_scene.AllowAvsToEscapeGravity ? .03f : .1f);
                            //How much should we force them down?
                            float fudgeHeight = (_parent_scene.AllowAvsToEscapeGravity ? 80 : 30);
                            //We add the 30 so that gravity is resonably strong once they pass the min height
                            Multiplier *= tempPos.Z + fudgeHeight - _parent_scene.AvGravityHeight;

                            //Limit these so that things don't go wrong
                            if (Multiplier < 1)
                                Multiplier = 1;

                            float maxpower = (_parent_scene.AllowAvsToEscapeGravity ? 1.5f : 3f);

                            if (Multiplier > maxpower)
                                Multiplier = maxpower;

                            _target_velocity.Z /= Multiplier;
                            vel.Z /= Multiplier;
                        }
                    }

                    #endregion

                    vec.Z = (_target_velocity.Z*movementmult - vel.Z)*PID_D*0.5f;
                    if (_parent_scene.AllowAvGravity && tempPos.Z > _parent_scene.AvGravityHeight)
                        //Add extra gravity
                        vec.Z += ((10*_parent_scene.gravityz)*Mass);
                }
            }

            if (realFlying)
            {
                #region Auto Fly Height

                //Added for auto fly height. Kitto Flora
                //Changed to only check if the avatar is flying around,

                // Revolution: If the avatar is going down, they are trying to land (probably), so don't push them up to make it harder
                //   Only if they are moving around sideways do we need to push them up
                if (_target_velocity.X != 0 || _target_velocity.Y != 0)
                {
                    Vector3 forwardVel = new Vector3(_target_velocity.X > 0 ? 2 : (_target_velocity.X < 0 ? -2 : 0),
                                                     _target_velocity.Y > 0 ? 2 : (_target_velocity.Y < 0 ? -2 : 0),
                                                     0);
                    float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y) +
                                            MinimumGroundFlightOffset;

                    //We cheat a bit and do a bit lower than normal
                    if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude ||
                        (tempPos.Z - CAPSULE_LENGTH) <
                        _parent_scene.GetTerrainHeightAtXY(tempPos.X + forwardVel.X, tempPos.Y + forwardVel.Y)
                        + MinimumGroundFlightOffset)
                        if (_target_velocity.Z < 0)
                            vec.Z += (target_altitude - tempPos.Z)*PID_P*0.5f; //Don't apply so much
                        else
                            vec.Z += (target_altitude - tempPos.Z)*PID_P*1.05f;
                }
                else
                {
                    //Straight up and down, only apply when they are very close to the ground
                    float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y);

                    if ((tempPos.Z - CAPSULE_LENGTH + (MinimumGroundFlightOffset/1.5)) <
                        target_altitude + MinimumGroundFlightOffset)
                    {
                        if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude + 1)
                        {
                            vec.Z += ((target_altitude + 4) - (tempPos.Z - CAPSULE_LENGTH))*PID_P;
                        }
                        else
                            vec.Z += ((target_altitude + MinimumGroundFlightOffset) - (tempPos.Z - CAPSULE_LENGTH))*
                                     PID_P*0.5f;
                    }
                }

                #endregion
            }

            #region Gravity

            if (!flying)
                _parent_scene.CalculateGravity(m_mass, tempPos, true, 1.0f, ref gravForce);
            else
                _parent_scene.CalculateGravity(m_mass, tempPos, false, 0.65f, ref gravForce);
                    //Allow point gravity and repulsors affect us a bit

            vec += gravForce;

            #endregion

            #region Under water physics

            if (_parent_scene.AllowUnderwaterPhysics && tempPos.X < _parent_scene.Region.RegionSizeX &&
                tempPos.Y < _parent_scene.Region.RegionSizeY)
            {
                //Position plus height to av's shoulder (aprox) is just above water
                if ((tempPos.Z + (CAPSULE_LENGTH/3) - .25f) < _parent_scene.GetWaterLevel(tempPos.X, tempPos.Y))
                {
                    if (StartingUnderWater)
                        ShouldBeWalking = Flying = false;
                    StartingUnderWater = false;
                    WasUnderWater = true;
                    Flying = true;
                    lastUnderwaterPush = 0;
                    if (ShouldBeWalking)
                    {
                        lastUnderwaterPush += (float) (_parent_scene.GetWaterLevel(tempPos.X, tempPos.Y) - tempPos.Z)*33 +
                                              3;
                        vec.Z += lastUnderwaterPush;
                    }
                    else
                    {
                        lastUnderwaterPush += 3500;
                        lastUnderwaterPush += (float) (_parent_scene.GetWaterLevel(tempPos.X, tempPos.Y) - tempPos.Z)*8;
                        vec.Z += lastUnderwaterPush;
                    }
                }
                else
                {
                    StartingUnderWater = true;
                    if (WasUnderWater)
                    {
                        WasUnderWater = false;
                        Flying = true;
                    }
                }
            }

            #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
        }
示例#11
0
        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;
                fy += m_force.Y;
                fz += m_force.Z;

                //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
                if (fx != 0 || fy != 0 || fz != 0)
                {
                    //m_taintdisable = true;
                    //base.RaiseOutOfBounds(Position);
                    //d.BodySetLinearVel(Body, fx, fy, 0f);
                    if (!d.BodyIsEnabled(Body))
                    {
                        d.BodySetLinearVel(Body, 0f, 0f, 0f);
                        d.BodySetForce(Body, 0, 0, 0);
                        enableBodySoft();
                    }
                    d.BodyAddForce(Body, fx, fy, fz);
                }
            }
            else
            {
                // _zeroPosition = d.BodyGetPosition(Body);
                return;
            }
        }
示例#12
0
        } // 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

            //Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well
            Vector3 PreferredAxisOfMotion = new Vector3 ((10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale) * pTimestep), 0, 0);
            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 > 90)
                {
                    // 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 * pTimestep * 16f);
                    angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep * pTimestep * 4f);
                    angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep * pTimestep * 4f);
                    

                    // velocity may still be acheived.

                    Vector3 dir = Vector3.One * rotq;
                    float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1);//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!

                    float mix = Math.Abs(m_bankingMix);
                    banking.Z += (effSquared * (mult * mix)) * (angularMotorVelocity.X);
                    m_angularMotorVelocity.X *= 1 - m_bankingEfficiency;
                    if(!parent.LinkSetIsColliding/* && Math.Abs(angularMotorVelocity.X) > mix*/) //If they are colliding, we probably shouldn't shove the prim around... probably
                    {
                        float angVelZ = angularMotorVelocity.X * -1;
                        /*if(angVelZ > mix)
                            angVelZ = mix;
                        else if(angVelZ < -mix)
                            angVelZ = -mix;*/
                        //This controls how fast and how far the banking occurs
                        Vector3 bankingRot = new Vector3(angVelZ * (effSquared * mult), 0, 0);
                        if(bankingRot.X > 3)
                            bankingRot.X = 3;
                        else if(bankingRot.X < -3)
                            bankingRot.X = -3;
                        bankingRot *= rotq;
                        banking += bankingRot;
                    }
                }
            }

            #endregion

            #region Downward Force

            Vector3 downForce = Vector3.Zero;

            double Zchange = m_lastposChange.Z;
            if((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) //if it isn't going up, don't apply the limiting force
            {
                if(Zchange < -0.1f)
                {
                    if(Zchange < -0.3f)
                        Zchange = -0.3f;
                    //Requires idea of 'up', so use reference frame to rotate it
                    //Add to the X, because that will normally tilt the vehicle downward (if its rotated, it'll be rotated by the ref. frame
                    downForce = (new Vector3(0, ((float)Math.Abs(Zchange) * (pTimestep * _pParentScene.PID_P / 4)), 0));
                    downForce *= rotq;
                }
            }

            #endregion

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

            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);
                if(float.IsNaN(torqueFromOffset.X))
                    torqueFromOffset.X = 0;
                if(float.IsNaN(torqueFromOffset.Y))
                    torqueFromOffset.Y = 0;
                if(float.IsNaN(torqueFromOffset.Z))
                    torqueFromOffset.Z = 0;
                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);
            if(parent.LinkSetIsColliding)
            {
                decayamount *= 100;
                if(decayamount.X > 1)
                    decayamount.X = 1;
                if(decayamount.Y > 1)
                    decayamount.Y = 1;
                if(decayamount.Z > 1)
                    decayamount.Z = 1;
            }
            m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;

            // Apply to the body

            if(m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.1f))
            {
                m_lastAngularVelocity = Vector3.Zero;
                d.BodySetAngularVel(Body, 0, 0, 0);
                m_angularZeroFlag = true;
            }
            else
            {
                d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
                m_angularZeroFlag = false;
            }
        }
示例#13
0
        }   // end Step

        private void MoveLinear (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
        {
            Vector3 motorDirection = m_linearMotorDirection;
            if(!motorDirection.ApproxEquals(Vector3.Zero, 0.01f) || m_linearMotorApply > 90)  // requested m_linearMotorDirection is significant
            {
                if(m_linearMotorApply <= 80)
                    if(m_linearMotorTimescale > 1)
                        m_linearMotorDirection /= m_linearMotorTimescale;
                    else
                    {
                        m_linearMotorDirection *= m_linearMotorTimescale;
                        motorDirection *= m_linearMotorTimescale;
                    }
                if (!d.BodyIsEnabled (Body))
                    d.BodyEnable (Body);

                //Interpolate between the current and last
                float diff = 100 - m_linearMotorApply;
                if(m_linearMotorApply >= 90)
                    motorDirection = (m_linearMotorDirection * (diff / 10f)) + (m_linearMotorDirectionLASTSET * (1 - (diff / 10f)));

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

                //This is a huge problem with the Bwind script, it 'must' be disabled
                // 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_linearMotorDirection.X = m_linearMotorDirectionLASTSET.X;
                if (Math.Abs (m_lastLinearVelocityVector.Y) > Math.Abs (m_linearMotorDirectionLASTSET.Y))
                    m_linearMotorDirection.Y = m_linearMotorDirectionLASTSET.Y;
                if (Math.Abs (m_lastLinearVelocityVector.Z) > Math.Abs (m_linearMotorDirectionLASTSET.Z))
                    m_linearMotorDirection.Z = m_linearMotorDirectionLASTSET.Z;*/

                if(!addAmount.ApproxEquals(Vector3.Zero, 0.01f))
                {
                    // decay applied velocity
                    float decayTime = m_linearMotorDecayTimescale / pTimestep;
                    Vector3 decayfraction = Vector3.One / decayTime;
                    if(decayfraction.X > 0.9f)
                        decayfraction.X = 0.9f;
                    if(decayfraction.Y > 0.9f)
                        decayfraction.Y = 0.9f;
                    if(decayfraction.Z > 0.9f)
                        decayfraction.Z = 0.9f;
                    Vector3 decayAmt = (motorDirection * decayfraction);
                    motorDirection -= decayAmt;
                    decayAmt = (m_linearMotorDirection * decayfraction);
                    m_linearMotorDirection -= decayAmt;
                }
                if(m_linearMotorApply > 0)
                    m_linearMotorApply--;
            }
            else if(m_linearMotorApply > 0)
                m_linearMotorApply--;

            // 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 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
                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;

            Vector3 pos = parent.Position;
            //            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);
            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)
            {
                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;
                }

                float tempHoverHeight = m_VhoverTargetHeight;
                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
                {
                    // If body is aready heigher, use its height as target height
                    if(pos.Z > tempHoverHeight)
                        tempHoverHeight = pos.Z;
                }
                if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                {
                    if((pos.Z - tempHoverHeight) > .2 || (pos.Z - tempHoverHeight) < -.2)
                    {
                        float h = tempHoverHeight;
                        float groundHeight = _pParentScene.GetTerrainHeightAtXY (pos.X, pos.Y);
                        if(groundHeight >= tempHoverHeight)
                            h = groundHeight;

                        d.BodySetPosition(Body, pos.X, pos.Y, tempHoverHeight);
                    }
                }
                else
                {
                    float herr0 = pos.Z - tempHoverHeight;
                    // Replace Vertical speed with correction figure if significant
                    if (herr0 > 0.01f)
                    {
                        m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
                        //KF: m_VhoverEfficiency is not yet implemented
                    }
                    else if(herr0 < -0.01f)
                    {
                        m_dir.Z = -((herr0 * pTimestep * 50f) / m_VhoverTimescale);
                    }
                    else
                    {
                        m_dir.Z = 0f;
                    }
                }

                //                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
                //                m_VhoverTimescale = 0f;        // time to acheive height
                //                pTimestep  is time since last frame,in secs
            }

            #endregion

            #region No X,Y,Z

            if ((m_flags & (VehicleFlag.NO_X)) != 0)
                m_dir.X = 0;
            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
                m_dir.Y = 0;
            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
                m_dir.Z = 0;

            #endregion

            #region Deal with tainted forces

            // 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.
            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
            Vector3 TaintedForce = new Vector3 ();
            if (m_forcelist.Count != 0)
            {
                try
                {
                    for (int i = 0; i < m_forcelist.Count; i++)
                    {
                        TaintedForce = TaintedForce + (m_forcelist[i]);
                    }
                }
                catch (IndexOutOfRangeException)
                {
                    TaintedForce = Vector3.Zero;
                }
                catch (ArgumentOutOfRangeException)
                {
                    TaintedForce = Vector3.Zero;
                }
                m_forcelist = new List<Vector3> ();
            }

            #endregion

            #region Deflection

            //Forward is the prefered direction
            /*Vector3 deflectionamount = m_dir / (m_linearDeflectionTimescale / pTimestep);
            //deflectionamount *= m_linearDeflectionEfficiency;
            if (deflectionamount != Vector3.Zero)
            {
            }
            Vector3 deflection = Vector3.One / deflectionamount;
            m_dir /= deflection;*/

            #endregion

            #region limitations

            if (Math.Abs (m_dir.X) > 1000 ||
                Math.Abs (m_dir.Y) > 1000 ||
                Math.Abs (m_dir.Z) > 1000)
            {
                m_dir = Vector3.Zero;
                /*
                //This vehicle is f***ed
                parent.RaiseOutOfBounds (parent.Position);
                parent._zeroFlag = true;
                parent.m_disabled = true;
                parent.m_frozen = true;*/
                return;
            }

            #endregion

            if(m_dir.ApproxEquals(Vector3.Zero, 0.001f))
                m_dir = Vector3.Zero;
            m_dir += TaintedForce;

            m_lastPositionVector = parent.Position;
            // Apply velocity
            d.BodySetLinearVel (Body, m_dir.X, m_dir.Y, m_dir.Z);
            // apply gravity force
            d.BodyAddForce (Body, grav.X, grav.Y, grav.Z);


            // apply friction
            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / (pTimestep));
            /*if(parent.LinkSetIsColliding)
            {
            }*/
            if(m_linearMotorDirectionLASTSET.X != 0 && (m_lastLinearVelocityVector.X / m_linearMotorDirectionLASTSET.X) < 10)
                decayamount.X += 0.025f * (m_lastLinearVelocityVector.X / m_linearMotorDirectionLASTSET.X);
            if(m_linearMotorDirectionLASTSET.Y != 0 && (m_lastLinearVelocityVector.Y / m_linearMotorDirectionLASTSET.Y) < 10)
                decayamount.Y += 0.025f * (m_lastLinearVelocityVector.Y / m_linearMotorDirectionLASTSET.Y);
            if(m_linearMotorDirectionLASTSET.Z != 0 && (m_lastLinearVelocityVector.Z / m_linearMotorDirectionLASTSET.Z) < 10)
                decayamount.Z += 0.025f * (m_lastLinearVelocityVector.Z / m_linearMotorDirectionLASTSET.Z);

            if(m_linearMotorApply <= 0)
                decayamount += new Vector3(0.1f, 0.1f, 0.1f);
            if(decayamount.X > 1)
                decayamount.X = 1;
            if(decayamount.Y > 1)
                decayamount.Y = 1;
            if(decayamount.Z > 1)
                decayamount.Z = 1;
            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
            if(m_linearMotorApply <= 0 ? m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.1f) :
                m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.001f))
            {
                m_lastLinearVelocityVector = Vector3.Zero;
                m_linearZeroFlag = true;
            }
            else
            {
                m_linearZeroFlag = false;
            }
        } // end MoveLinear()
示例#14
0
        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))
                            {
                            //  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)));
                            }
                        }        // end if (m_usePID)
                    #endregion
                    #region Hover PID
                    // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
                    if (m_useHoverPID && !m_usePID)
                        {
                        //Console.WriteLine("Hover " +  m_primName);

                        // If we're using the PID controller, then we have no gravity
                        fx = -_parent_scene.gravityx;
                        fy = -_parent_scene.gravityy;
                        fz = -_parent_scene.gravityz;

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


                        // Where are we, and where are we headed?

                        //    Non-Vehicles have a limited set of Hover options.
                        // determine what our target height really is based on HoverType
                        switch (m_PIDHoverType)
                            {
                            case PIDHoverType.Ground:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY((float)dcpos.X, (float)dcpos.Y);
                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                break;
                            case PIDHoverType.GroundAndWater:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY((float)dcpos.X, (float)dcpos.Y);
                                m_waterHeight = (float)_parent_scene.GetWaterLevel((float)dcpos.X, (float)dcpos.Y);
                                if (m_groundHeight > m_waterHeight)
                                    {
                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                    }
                                else
                                    {
                                    m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
                                    }
                                break;

                            }     // end switch (m_PIDHoverType)


                        _target_velocity =
                            new Vector3(0.0f, 0.0f,
                                (float)(m_targetHoverHeight - dcpos.Z) * ((PID_G - m_PIDHoverTau) * 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

                            d.BodySetPosition(prim_geom, dcpos.X, dcpos.Y, m_targetHoverHeight);
                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
                            d.BodyAddForce(Body, 0, 0, fz);
                            return;
                            }
                        else
                            {
                            _zeroFlag = false;

                            // We're flying and colliding with something
                            fz = (float)(fz + ((_target_velocity.Z - vel.Z) * (PID_D)));
                            }
                        }
                    #endregion

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

                    fx += m_force.X;
                    fy += m_force.Y;
                    fz += m_force.Z;

                    # region drag and forces accumulators

                    float drag = -m_mass * 0.2f;

                    fx += drag * vel.X;
                    fy += drag * vel.Y;
                    fz += drag * vel.Z;

                   
                    Vector3 newtorque;
                    newtorque.X = m_angularforceacc.X;
                    newtorque.Y = m_angularforceacc.Y;
                    newtorque.Z = m_angularforceacc.Z;
                    m_angularforceacc = Vector3.Zero;

                    fx += m_forceacc.X;
                    fy += m_forceacc.Y;
                    fz += m_forceacc.Z;
                    m_forceacc = Vector3.Zero;

                    #endregion

                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
                    if (fx != 0 || fy != 0 || fz != 0 || newtorque.X != 0 || newtorque.Y != 0 || newtorque.Z != 0)
                        {
                        // 35n times the mass per second applied maximum.
                        float nmax = 35f * m_mass;
                        float nmin = -35f * m_mass;

                        if (fx > nmax)
                            fx = nmax;
                        if (fx < nmin)
                            fx = nmin;
                        if (fy > nmax)
                            fy = nmax;
                        if (fy < nmin)
                            fy = nmin;

                        if (Amotor != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0.003f)                                )
                            {
                            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.0001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.0001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.0001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.0001f);
                            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.0001f);
                            }
/*
                        if (vel.Z < -30)
                            {
                            vel.Z = -30;
                            }
*/
                        bool disabled = false;
/*
                        if (_parent_scene.m_DisableSlowPrims)
                            {
                            if (((float)fz == (float)(_parent_scene.gravityz * m_mass)) &&
                                (Math.Abs(vel.X) < 0.01 || Math.Abs(vel.Y) < 0.01 || Math.Abs(vel.Z) < 0.0001))
                                {
                                if (Math.Abs(vel.X) < 0.0001 || Math.Abs(vel.Y) < 0.0001 || Math.Abs(vel.Z) < 0.0001)
                                    {
                                    Vector3 angvelocity = new Vector3((float)angvel.X, (float)angvel.Y, (float)angvel.Z);

                                    if (angvelocity.ApproxEquals(Vector3.Zero, 0.005f) &&
                                        vel.X != 0 && vel.Y != 0 && vel.Z != 0)
                                        {
                                        if (d.BodyIsEnabled(Body))
                                            {
                                            d.BodySetLinearVel(Body, 0, 0, 0);
                                            d.BodySetForce(Body, 0, 0, 0);
                                            d.BodyDisable(Body);
                                            disabled = true;
                                            }
                                        }
                                    else
                                        {
                                        if (!d.BodyIsEnabled(Body))
                                            d.BodyEnable(Body);
                                        }
                                    }
                                else
                                    {
                                    if (!d.BodyIsEnabled(Body))
                                        {
                                        d.BodyEnable(Body);
                                        fz = 100 * m_mass;
                                        }
                                    }
                                }
                            }
*/
                        if (!disabled)
                            {
                            if (!d.BodyIsEnabled(Body))
                                {
                                enableBodySoft();                               
                                }

                            d.BodyAddForce(Body, fx, fy, fz);
                            d.BodyAddTorque(Body, newtorque.X, newtorque.Y, newtorque.Z);                    

                            }
                        }
                    }
                }
            else
                {    // is not physical, or is not a body or is selected
                //  _zeroPosition = d.BodyGetPosition(Body);
                return;
                //Console.WriteLine("Nothing " +  m_primName);

                }           
            }
示例#15
0
 public virtual bool ErrorIsZero(Vector3 err)
 {
     return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
 }
示例#16
0
        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;
        }
示例#17
0
        // =======================================================================
        // =======================================================================
        // Apply the effect of the angular motor.
        // The 'contribution' is how much angular correction velocity each function wants.
        //     All the contributions are added together and the resulting velocity is
        //     set directly on the vehicle.
        private void MoveAngular(float pTimestep)
        {
            // The user wants this many radians per second angular change?
            Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);

            // ==================================================================
            // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
            //    This flag prevents linear deflection parallel to world z-axis. This is useful
            //    for preventing ground vehicles with large linear deflection, like bumper cars,
            //    from climbing their linear deflection into the sky. 
            // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
            {
                angularMotorContribution.X = 0f;
                angularMotorContribution.Y = 0f;
                VDetailLog("{0},  MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
            }

            Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();

            Vector3 deflectionContribution = ComputeAngularDeflection();

            Vector3 bankingContribution = ComputeAngularBanking();

            // ==================================================================
            m_lastVertAttractor = verticalAttractionContribution;

            m_lastAngularVelocity = angularMotorContribution
                                    + verticalAttractionContribution
                                    + deflectionContribution
                                    + bankingContribution;

            // ==================================================================
            // Apply the correction velocity.
            // TODO: Should this be applied as an angular force (torque)?
            if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
            {
                VehicleRotationalVelocity = m_lastAngularVelocity;

                VDetailLog("{0},  MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5}",
                                    Prim.LocalID,
                                    angularMotorContribution, verticalAttractionContribution,
                                    bankingContribution, deflectionContribution,
                                    m_lastAngularVelocity
                                    );
            }
            else
            {
                // The vehicle is not adding anything angular wise.
                VehicleRotationalVelocity = Vector3.Zero;
                VDetailLog("{0},  MoveAngular,done,zero", Prim.LocalID);
            }

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

                // TODO: this computation should be in the linear section
                //    because that is where we know the impulse being applied.
                Vector3 torqueFromOffset = Vector3.Zero;
                // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
                if (float.IsNaN(torqueFromOffset.X))
                    torqueFromOffset.X = 0;
                if (float.IsNaN(torqueFromOffset.Y))
                    torqueFromOffset.Y = 0;
                if (float.IsNaN(torqueFromOffset.Z))
                    torqueFromOffset.Z = 0;

                VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
                VDetailLog("{0},  BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
            }

        }
示例#18
0
        } // end MoveLinear()

        private void MoveAngular(float pTimestep)
        {
            /*
            private Vector3 m_angularMotorDirection = Vector3.Zero;            // angular velocity requested by LSL motor
            private int m_angularMotorApply = 0;                            // application frame counter
             private float m_angularMotorVelocity = 0;                        // current angular motor velocity (ramps up and down)
            private float m_angularMotorTimescale = 0;                        // motor angular velocity ramp up rate
            private float m_angularMotorDecayTimescale = 0;                    // motor angular velocity decay rate
            private Vector3 m_angularFrictionTimescale = Vector3.Zero;        // body angular velocity  decay rate
            private Vector3 m_lastAngularVelocity = Vector3.Zero;            // what was last applied to body
            */

            // Get what the body is doing, this includes 'external' influences
            d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
   //         Vector3 angularVelocity = Vector3.Zero;

            if (m_angularMotorApply > 0)
            {
                // 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);
                m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);

                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
                                            // velocity may still be acheived.
            }
            else
            {
                // 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);
            } // end motor section

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

            if (m_verticalAttractionTimescale < 300)
            {
                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
                // get present body rotation
                d.Quaternion rot = d.BodyGetQuaternion(Body);
                Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
                // 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

    //        m_lastVertAttractor = vertattr;

            // Bank section tba
            // Deflection section tba

            // Sum velocities
            m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
            
            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
            {
                m_lastAngularVelocity.X = 0;
                m_lastAngularVelocity.Y = 0;
            }

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

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

        } //end MoveAngular
示例#19
0
    // The user sets all the parameters and calls this which outputs values until error is zero.
    public override void GenerateTestOutput(float timeStep)
    {
        // maximum number of outputs to generate.
        int maxOutput = 50;
        MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
        MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}",
                                BSScene.DetailLogZero, UseName,
                                TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency, 
                                CurrentValue, TargetValue);

        LastError = BSMotor.InfiniteVector;
        while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
        {
            Vector3 lastStep = Step(timeStep);
            MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
                            BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
        }
        MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
        

    }
示例#20
0
 private void changeAngularLock(Vector3 newlock)
 {
     // do we have a Physical object?
     if (Body != IntPtr.Zero)
     {
         //Check that we have a Parent
         //If we have a parent then we're not authorative here
         if (_parent == null)
         {
             if (!newlock.ApproxEquals(Vector3.One, 0f))
             {
                 createAMotor(newlock);
             }
             else
             {
                 if (Amotor != IntPtr.Zero)
                 {
                     d.JointDestroy(Amotor);
                     Amotor = IntPtr.Zero;
                 }
             }
         }
     }
     // Store this for later in case we get turned into a separate body
     m_angularlock = newlock;
 }
示例#21
0
        /// <summary>
        /// Called from Simulate
        /// This is the avatar's movement control + PID Controller
        /// </summary>
        /// <param name="timeStep"></param>
        public void Move(float timeStep, 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;

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

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

            // rex, added height check

            d.Vector3 tempPos = 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


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

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

            #region Check for underground

//            _parent_scene.CheckTerrainColisionAABB(Shell);


            //            if (!flying || (flying && _target_velocity.X == 0 || _target_velocity.Y == 0))
            //            if (!m_iscollidingGround)
            //Don't duplicate the ground check for flying from above, it will already have given us a good shove
                {
                //                if (m_WaitGroundCheck >= 10 && vel.Z != 0)
                    {
                    float groundHeight = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y);
                    if ((tempPos.Z - AvatarHalfsize) < groundHeight)
                        {
                        if (!flying)
                            {
                            if (_target_velocity.Z < 0)
                                _target_velocity.Z = 0;
                            vec.Z = -vel.Z * PID_D + ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P * 20.0f);
                            }
                        else
                            vec.Z = ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P);
                        }
                    if (tempPos.Z - AvatarHalfsize - groundHeight < 0.1)
                        {
                        m_iscolliding = true;
                        m_iscollidingGround = true;
                        flying = false; // gound the avatar
                        }
                    else
                        m_iscollidingGround = false;


                    //                    m_WaitGroundCheck = -1;
                    }
                //                m_WaitGroundCheck++;
                }

/*
            if (!m_alwaysRun)
                movementdivisor = _parent_scene.avMovementDivisorWalk * (_parent_scene.TimeDilation < 0.3 ? 0.6f : _parent_scene.TimeDilation); //Dynamically adjust it for slower sims
            else
                movementdivisor = _parent_scene.avMovementDivisorRun * (_parent_scene.TimeDilation < 0.3 ? 0.6f : _parent_scene.TimeDilation); //Dynamically adjust it for slower sims
*/
            // no dinamic messing here

            float movementmult = 1f;
            if (!m_alwaysRun)
                movementmult /= _parent_scene.avMovementDivisorWalk;
            else
                movementmult /= _parent_scene.avMovementDivisorRun;


            //  if velocity is zero, use position control; otherwise, velocity control
            if (_target_velocity == Vector3.Zero &&
                Math.Abs(vel.X) < 0.05 && Math.Abs(vel.Y) < 0.05 && Math.Abs(vel.Z) < 0.05 && (this.m_iscollidingGround || this.m_iscollidingObj || this.flying))
                //This is so that if we get moved by something else, it will update us in the client
                {
                //  keep track of where we stopped.  No more slippin' & slidin'
                if (!_zeroFlag)
                    {
                    _zeroFlag = true;
                    _zeroPosition = tempPos;
                    }

                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
                    // if target vel is zero why was it here ?
                    //vec.X = -vel.X * PID_D + (_zeroPosition.X - tempPos.X) * PID_P * 2f;
                    //vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - tempPos.Y) * PID_P * 2f;
                    }
                }
            else
                {
                m_pidControllerActive = true;
                _zeroFlag = false;

                if (m_iscolliding)
                    {
                    if(!flying)
                        {
                        if (_target_velocity.Z != 0.0f)
                            vec.Z = (_target_velocity.Z - vel.Z) * PID_D;// + (_zeroPosition.Z - tempPos.Z) * PID_P)) _zeropos maybe bad here
                        // We're standing or walking on something
                        vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D*2;
                        vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D*2;
                        }
                    else 
                        {
                    // We're flying and colliding with something
                        vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.5f;
                        vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.5f;
                        }
                    }               
                else
                    {
                    if (flying)
                        {
                        // we're flyind
                        vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.75f;
                        vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.75f;
                        }

                    else 
                        {
                        // we're not colliding and we're not flying so that means we're falling!
                        // m_iscolliding includes collisions with the ground.
                        vec.X = (_target_velocity.X - vel.X) * PID_D * 0.85f;
                        vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.85f;
                        }
                    }

                if (flying)
                    {
                    #region Av gravity

                    if (_parent_scene.AllowAvGravity &&
                        tempPos.Z > _parent_scene.AvGravityHeight) //Should be stop avies from flying upwards
                        {
                        //Decay going up 
                        if (_target_velocity.Z > 0)
                            {
                            //How much should we force them down?
                            float Multiplier = (_parent_scene.AllowAvsToEscapeGravity ? .03f : .1f);
                            //How much should we force them down?
                            float fudgeHeight = (_parent_scene.AllowAvsToEscapeGravity ? 80 : 30);
                            //We add the 30 so that gravity is resonably strong once they pass the min height
                            Multiplier *= tempPos.Z + fudgeHeight - _parent_scene.AvGravityHeight;

                            //Limit these so that things don't go wrong
                            if (Multiplier < 1)
                                Multiplier = 1;

                            float maxpower = (_parent_scene.AllowAvsToEscapeGravity ? 1.5f : 3f);

                            if (Multiplier > maxpower)
                                Multiplier = maxpower;

                            _target_velocity.Z /= Multiplier;
                            vel.Z /= Multiplier;
                            }
                        }

                    #endregion

                    vec.Z = (_target_velocity.Z - vel.Z) * PID_D * 0.5f;
                    if (_parent_scene.AllowAvGravity && tempPos.Z > _parent_scene.AvGravityHeight)
                        //Add extra gravity
                        vec.Z += ((10 * _parent_scene.gravityz) * Mass);
                    }
                }

            if (flying)
                {
                #region Auto Fly Height

                //Added for auto fly height. Kitto Flora
                //Changed to only check if the avatar is flying around,

                // Revolution: If the avatar is going down, they are trying to land (probably), so don't push them up to make it harder
                //   Only if they are moving around sideways do we need to push them up
                if (_target_velocity.X != 0 || _target_velocity.Y != 0)
                    {
                    Vector3 forwardVel = new Vector3(_target_velocity.X > 0 ? 2 : (_target_velocity.X < 0 ? -2 : 0),
                        _target_velocity.Y > 0 ? 2 : (_target_velocity.Y < 0 ? -2 : 0),
                        0);
                    float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y) + MinimumGroundFlightOffset;

                    //We cheat a bit and do a bit lower than normal
                    if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude ||
                            (tempPos.Z - CAPSULE_LENGTH) < _parent_scene.GetTerrainHeightAtXY(tempPos.X + forwardVel.X, tempPos.Y + forwardVel.Y)
                            + MinimumGroundFlightOffset)
                        vec.Z += (target_altitude - tempPos.Z) * PID_P * 0.5f;
                    }
                else
                    {
                    //Straight up and down, only apply when they are very close to the ground
                    float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y);

                    if ((tempPos.Z - CAPSULE_LENGTH + (MinimumGroundFlightOffset / 1.5)) < target_altitude + MinimumGroundFlightOffset)
                        {
                        if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude + 1)
                            {
                            vec.Z += ((target_altitude + 4) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P;
                            }
                        else
                            vec.Z += ((target_altitude + MinimumGroundFlightOffset) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P * 0.5f;
                        }
                    }

                #endregion
            }

            #region Gravity

            if (!flying && _parent_scene.AllowAvGravity)
                {
                if (!_parent_scene.UsePointGravity)
                    {
                    //Add normal gravity
                    vec.X += _parent_scene.gravityx * m_mass;
                    vec.Y += _parent_scene.gravityy * m_mass;
                    vec.Z += _parent_scene.gravityz * m_mass;
                    }
                else
                    {
                    Vector3 cog = _parent_scene.PointOfGravity;
                    if (cog.X != 0)
                        vec.X += (cog.X - tempPos.X) * m_mass;
                    if (cog.Y != 0)
                        vec.Y += (cog.Y - tempPos.Y) * m_mass;
                    if (cog.Z != 0)
                        vec.Z += (cog.Z - tempPos.Z) * m_mass;
                    }
                }

            #endregion

            #region Under water physics

            if (_parent_scene.AllowUnderwaterPhysics)
                {
                //Position plus height to av's shoulder (aprox) is just above water
                    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

            if (vec.IsFinite())
            {
                if (vec.X < 100000000 && vec.Y < 10000000 && vec.Z < 10000000) //Checks for crazy, going to NaN us values
                {
                    d.Vector3 veloc = d.BodyGetLinearVel(Body);
                    //Stop us from fidgiting if we have a small velocity
                    /*
                                        if (_zeroFlag && ((Math.Abs(vec.X) < 0.09 && Math.Abs(vec.Y) < 0.09 && Math.Abs(vec.Z) < 0.03) && !flying && vec.Z != 0))
                                        {
                                            //m_log.Warn("Nulling Velo: " + vec.ToString());
                                            vec = new Vector3(0, 0, 0);
                                            d.BodySetLinearVel(Body, 0, 0, 0);
                                        }

                                        //Reduce insanely small values to 0 if the velocity isn't going up
                                        if (Math.Abs(vec.Z) < 0.01 && veloc.Z < 0.6 && _zeroFlag)
                                        {
                                            if (veloc.Z != 0)
                                            {
                                                if (-veloc.Z > 0)
                                                    vec.Z = 0;
                                                else
                                                    vec.Z = -veloc.Z * 5;
                                                d.BodySetLinearVel(Body, veloc.X, veloc.Y, vec.Z);
                                            }
                                        }

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


                    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;

                    //Check if the capsule is tilted before changing it
//                    if (!_zeroFlag && !_parent_scene.IsAvCapsuleTilted)
//                        AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
                }
                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);
                // _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;
                    }
                }
            }
示例#22
0
        public void Move(float timestep)
        {
            float fx = 0;
            float fy = 0;
            float fz = 0;

            if (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(timestep, _parent_scene);
                }
                else
                {
//Console.WriteLine("Move " +  Name);
                    if (!d.BodyIsEnabled (Body))  d.BodyEnable (Body); // KF add 161009
                    // NON-'VEHICLES' are dealt with here
//                    if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
//                    {
//                        d.Vector3 avel2 = d.BodyGetAngularVel(Body);
//                        /*
//                        if (m_angularlock.X == 1)
//                            avel2.X = 0;
//                        if (m_angularlock.Y == 1)
//                            avel2.Y = 0;
//                        if (m_angularlock.Z == 1)
//                            avel2.Z = 0;
//                        d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
//                         */
//                    }
                    //float PID_P = 900.0f;

                    float m_mass = CalculateMass();

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

                    
                    //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
                    fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;

                    if (m_usePID)
                    {
//Console.WriteLine("PID " +  Name);
                        // 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();
                        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);
                        }
                    }        // end if (m_usePID)

                    // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
                    if (m_useHoverPID && !m_usePID)
                    {
//Console.WriteLine("Hover " +  Name);
                    
                        // 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;
                        }

                        // Where are we, and where are we headed?
                        d.Vector3 pos = d.BodyGetPosition(Body);
                        d.Vector3 vel = d.BodyGetLinearVel(Body);

                        //    Non-Vehicles have a limited set of Hover options.
                        // determine what our target height really is based on HoverType
                        switch (m_PIDHoverType)
                        {
                            case PIDHoverType.Ground:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                break;
                            case PIDHoverType.GroundAndWater:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
                                m_waterHeight  = _parent_scene.GetWaterLevel();
                                if (m_groundHeight > m_waterHeight)
                                {
                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                }
                                else
                                {
                                    m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
                                }
                                break;

                        }     // end switch (m_PIDHoverType)


                        _target_velocity =
                            new Vector3(0.0f, 0.0f,
                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * 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

                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
                            d.BodyAddForce(Body, 0, 0, fz);
                            return;
                        }
                        else
                        {
                            _zeroFlag = false;

                            // We're flying and colliding with something
                            fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
                        }
                    }

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

                    fx += m_force.X;
                    fy += m_force.Y;
                    fz += m_force.Z;

                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
                    if (fx != 0 || fy != 0 || fz != 0)
                    {
                        //m_taintdisable = true;
                        //base.RaiseOutOfBounds(Position);
                        //d.BodySetLinearVel(Body, fx, fy, 0f);
                        if (!d.BodyIsEnabled(Body))
                        {
                            // A physical body at rest on a surface will auto-disable after a while,
                            // this appears to re-enable it incase the surface it is upon vanishes,
                            // and the body should fall again. 
                            d.BodySetLinearVel(Body, 0f, 0f, 0f);
                            d.BodySetForce(Body, 0, 0, 0);
                            enableBodySoft();
                        }

                        // 35x10 = 350n times the mass per second applied maximum.
                        float nmax = 35f * m_mass;
                        float nmin = -35f * m_mass;
                    
                        if (fx > nmax)
                            fx = nmax;
                        if (fx < nmin)
                            fx = nmin;
                        if (fy > nmax)
                            fy = nmax;
                        if (fy < nmin)
                            fy = nmin;
                        d.BodyAddForce(Body, fx, fy, fz);
//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
                    }
                }
            }
            else
            {    // is not physical, or is not a body or is selected
              //  _zeroPosition = d.BodyGetPosition(Body);
                return;
//Console.WriteLine("Nothing " +  Name);
               
            }
        }
示例#23
0
        } // end MoveLinear()

        // Apply the effect of the angular motor.
        private void MoveAngular(float pTimestep)
        {
            // m_angularMotorDirection         // angular velocity requested by LSL motor
            // m_angularMotorApply             // application frame counter
            // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
            // m_angularMotorTimescale         // motor angular velocity ramp up rate
            // m_angularMotorDecayTimescale    // motor angular velocity decay rate
            // m_angularFrictionTimescale      // body angular velocity  decay rate
            // m_lastAngularVelocity           // what was last applied to body

            // Get what the body is doing, this includes 'external' influences
            Vector3 angularVelocity = Prim.ForceRotationalVelocity;

            if (m_angularMotorApply > 0)
            {
                // Rather than snapping the angular motor velocity from the old value to
                //    a newly set velocity, this routine steps the value from the previous
                //    value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
                // There are m_angularMotorApply steps.
                Vector3 origVel = m_angularMotorVelocity;
                Vector3 origDir = m_angularMotorDirection;

                // ramp up to new value
                //       new 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);
                m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);

                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},origDir={5},vel={6}",
                        Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);

                m_angularMotorApply--;
            }
            else
            {
                // No motor recently applied, keep the body velocity
                // and decay the velocity
                m_angularMotorVelocity -= m_angularMotorVelocity /  (m_angularMotorDecayTimescale / pTimestep);
                if (m_angularMotorVelocity.LengthSquared() < 0.00001)
                    m_angularMotorVelocity = Vector3.Zero;
            } // end motor section

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

            if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
            {
                float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
                VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);

                // get present body rotation
                Quaternion rotq = Prim.ForceOrientation;
                // 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 amounts. 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.

                // Error is 0 (no error) to +/- 2 (max error)
                if (verterr.Z < 0.0f)
                {
                    verterr.X = 2.0f - verterr.X;
                    verterr.Y = 2.0f - verterr.Y;
                }
                // scale it by VAservo
                verterr = verterr * VAservo;

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

                VDetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}",
                            Prim.LocalID, verterr, bounce, vertattr);

            } // else vertical attractor is off

            m_lastVertAttractor = vertattr;

            // Bank section tba

            // Deflection section tba

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

            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
            {
                m_lastAngularVelocity.X = 0;
                m_lastAngularVelocity.Y = 0;
                VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
            }

            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
            {
                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
                VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
            }

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

            // Apply to the body
            Prim.ForceRotationalVelocity = m_lastAngularVelocity;

            VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity);
        } //end MoveAngular
示例#24
0
        public void Move(float timestep)
        {
            if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
                !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building)        // KF: Only move root prims.
            {
//                if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009

                float fx = 0;
                float fy = 0;
                float fz = 0;

                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
                {
                    // 'VEHICLES' are dealt with in ODEDynamics.cs
                    m_vehicle.Step(timestep, _parent_scene,Body);
                }
                else
                {
                     float m_mass = _mass;
                   
                    //                    fz = 0f;
                    //m_log.Info(m_collisionFlags.ToString());
                    if (m_usePID)
                    {
  
                        // 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;
                        }

                        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 = ((_target_velocity.Z - vel.Z) * (PID_D));
                        }
                    }        // end if (m_usePID)

                    // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
                    else if (m_useHoverPID)
                    {
                        //Console.WriteLine("Hover " +  Name);

                        // If we're using the PID controller, then we have no gravity

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

                        // Where are we, and where are we headed?
                        d.Vector3 pos = d.BodyGetPosition(Body);
                        d.Vector3 vel = d.BodyGetLinearVel(Body);

                        //    Non-Vehicles have a limited set of Hover options.
                        // determine what our target height really is based on HoverType
                        switch (m_PIDHoverType)
                        {
                            case PIDHoverType.Ground:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                break;
                            case PIDHoverType.GroundAndWater:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
                                m_waterHeight = _parent_scene.GetWaterLevel();
                                if (m_groundHeight > m_waterHeight)
                                {
                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                }
                                else
                                {
                                    m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
                                }
                                break;

                        }     // end switch (m_PIDHoverType)


                        _target_velocity =
                            new Vector3(0.0f, 0.0f,
                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * 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

                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
                            // ?                        d.BodyAddForce(Body, 0, 0, fz);
                            return;
                        }
                        else
                        {
                            _zeroFlag = false;

                            // We're flying and colliding with something
                            fz = ((_target_velocity.Z - vel.Z) * (PID_D));
                        }
                    }
                    else
                    {
                        float b = (1.0f - m_buoyancy);
                        fx = _parent_scene.gravityx * b;
                        fy = _parent_scene.gravityy * b;
                        fz = _parent_scene.gravityz * b;
                    }

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

                    // constant force
                    fx += m_force.X;
                    fy += m_force.Y;
                    fz += m_force.Z;

                    fx += m_forceacc.X;
                    fy += m_forceacc.Y;
                    fz += m_forceacc.Z;

                    m_forceacc = Vector3.Zero;
                    
                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
                    if (fx != 0 || fy != 0 || fz != 0)
                    {
                        d.BodyAddForce(Body, fx, fy, fz);
                        //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
                    }

                    Vector3 trq;

                    trq = _torque;
                    trq += m_angularForceacc;
                    m_angularForceacc = Vector3.Zero;
                    if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
                    {
                        d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
                    }

                }
            }
            else
            {    // is not physical, or is not a body or is selected
                //  _zeroPosition = d.BodyGetPosition(Body);
                return;
                //Console.WriteLine("Nothing " +  Name);

            }
        }
示例#25
0
        public void TestLinkDelink2SceneObjects()
        {
            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;

            grp1.AbsolutePosition = new Vector3(10, 10, 10);
            grp2.AbsolutePosition = Vector3.Zero;

            // <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));
            
            // Required for linking
            grp1.RootPart.ClearUpdateSchedule();
            grp2.RootPart.ClearUpdateSchedule();

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

            // FIXME: Can't do this test yet since group 2 still has its root part!  We can't yet null this since
            // it might cause SOG.ProcessBackup() to fail due to the race condition.  This really needs to be fixed.
            Assert.That(grp2.IsDeleted, "SOG 2 was not registered as deleted after link.");
            Assert.That(grp2.Parts.Length, Is.EqualTo(0), "Group 2 still contained children after delink.");
            Assert.That(grp1.Parts.Length == 2);

            if (debugtest)
            {
                m_log.Debug("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);
            }

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

            // 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.");

            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 exactly sure what this is asserting...");

            // Delink part 2
            SceneObjectGroup grp3 = grp1.DelinkFromGroup(part2.LocalId);

            if (debugtest)
                m_log.Debug("Group2: Prim2: OffsetPosition:" + part2.AbsolutePosition + ", OffsetRotation:" + part2.RotationOffset);

            Assert.That(grp1.Parts.Length, Is.EqualTo(1), "Group 1 still contained part2 after delink.");
            Assert.That(part2.AbsolutePosition == Vector3.Zero, "The absolute position should be zero");
            Assert.That(grp3.HasGroupChangedDueToDelink, Is.True);
        }
示例#26
0
    public virtual Vector3 Step(float timeStep, Vector3 error)
    {
        if (!Enabled) return Vector3.Zero;

        LastError = error;
        Vector3 returnCorrection = Vector3.Zero;
        if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
        {
            // correction =  error / secondsItShouldTakeToCorrect
            Vector3 correctionAmount;
            if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
                correctionAmount = error * timeStep;
            else
                correctionAmount = error / TimeScale * timeStep;

            returnCorrection = correctionAmount;
            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
                                    BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
        }
        return returnCorrection;
    }