/// <summary>
 ///     Dereference the creator scene so that it can be garbage collected if needed.
 /// </summary>
 internal void Dispose()
 {
     m_scene = null;
     if (ContactgeomsArray != IntPtr.Zero)
         Marshal.FreeHGlobal(ContactgeomsArray);
 }
Exemplo n.º 2
0
        // end Step
        private void MoveLinear(float pTimestep, WhiteCoreODEPhysicsScene _pParentScene, WhiteCoreODEPrim parent)
        {
            if (m_linearMotorDirection.LengthSquared() < 0.0001f)
            {
                m_linearMotorDirection = Vector3.Zero;
                m_newVelocity = Vector3.Zero;
            }
            else
            {
                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale);
                m_lastLinearVelocityVector += (addAmount);

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

                // convert requested object velocity to world-referenced vector
                d.Quaternion rot = d.BodyGetQuaternion(Body);
                Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
                //Vector3 oldVelocity = m_newVelocity;
                m_newVelocity = m_lastLinearVelocityVector*rotq; // apply obj rotation to velocity vector
                //if (oldVelocity.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
                //    m_newVelocity.Z += dvel_now.Z; // Preserve the accumulated falling velocity
            }

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

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

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

            #region Blocking Change

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

            #endregion

            #region Terrain checks

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

            #endregion

            #region Hover

            // Check if hovering
            if ((m_flags &
                 (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, h);
                    }
                }
                else
                {
                    float herr0 = pos.Z - tempHoverHeight;
                    // Replace Vertical speed with correction figure if significant
                    if (herr0 > 0.01f)
                    {
                        m_newVelocity.Z = -((herr0*50.0f)/m_VhoverTimescale);
                        //KF: m_VhoverEfficiency is not yet implemented
                    }
                    else if (herr0 < -0.01f)
                    {
                        m_newVelocity.Z = -((herr0*50f)/m_VhoverTimescale);
                    }
                    else
                    {
                        m_newVelocity.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_newVelocity.X = 0;
            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
                m_newVelocity.Y = 0;
            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
                m_newVelocity.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
                {
                    TaintedForce = m_forcelist.Aggregate(TaintedForce, (current, t) => current + (t));
                }
                catch (IndexOutOfRangeException)
                {
                    TaintedForce = Vector3.Zero;
                }
                catch (ArgumentOutOfRangeException)
                {
                    TaintedForce = Vector3.Zero;
                }
                m_forcelist = new List<Vector3>();
            }

            // force to deltaV
            m_newVelocity += TaintedForce*(pTimestep/Mass);

            #endregion

            #region Deflection

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

            #endregion

            #region limitations

            if (m_newVelocity.LengthSquared() > 1e6f)
            {
                m_newVelocity /= m_newVelocity.Length();
                m_newVelocity *= 1000f;
            }
            else if (m_newVelocity.LengthSquared() < 1e-6f)
                m_newVelocity = Vector3.Zero;

            #endregion

            m_lastPositionVector = parent.Position;

            float grav = -1*Mass*pTimestep;

            // Apply velocity
            if (m_newVelocity != Vector3.Zero)
            {
                if ((Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED) && !parent.LinkSetIsColliding)
                {
                    //Force ODE gravity here!!!
                }
                else
                    d.BodySetLinearVel(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z + grav);
            }
            // apply friction
            m_lastLinearVelocityVector.X *= (1.0f - 1/m_linearFrictionTimescale.X);
            m_lastLinearVelocityVector.Y *= (1.0f - 1/m_linearFrictionTimescale.Y);
            m_lastLinearVelocityVector.Z *= (1.0f - 1/m_linearFrictionTimescale.Z);
        }
        public WhiteCoreODERayCastRequestManager(WhiteCoreODEPhysicsScene pScene)
        {
            m_scene = pScene;
            nearCallback = near;

            ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
        }
Exemplo n.º 4
0
        internal void Step(IntPtr pBody, float pTimestep, WhiteCoreODEPhysicsScene pParentScene, WhiteCoreODEPrim parent)
        {
            m_body = pBody;
            if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
                return;
            if (Mass == 0)
                GetMass(pBody);
            if (Mass == 0)
                return; //No noMass vehicles...
            if (!d.BodyIsEnabled(Body))
                d.BodyEnable(Body);

            frcount++; // used to limit debug comment output
            if (frcount > 100)
                frcount = 0;

            // scale time so parameters work as before
            // until we scale then acording to ode step time

            MoveLinear(pTimestep, pParentScene, parent);
            MoveAngular(pTimestep, pParentScene, parent);
            LimitRotation(pTimestep);
        }
Exemplo n.º 5
0
        // end MoveLinear()
        private void MoveAngular(float pTimestep, WhiteCoreODEPhysicsScene _pParentScene, WhiteCoreODEPrim 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_angularMotorDirection.LengthSquared() > 1e-6f)
            {
                m_angularMotorVelocity.X = (m_angularMotorDirection.X - angularVelocity.X)/(m_angularMotorTimescale);
                m_angularMotorVelocity.Y = (m_angularMotorDirection.Y - angularVelocity.Y)/(m_angularMotorTimescale);
                m_angularMotorVelocity.Z = (m_angularMotorDirection.Z - angularVelocity.Z)/(m_angularMotorTimescale);
                m_angularMotorDirection *= (1.0f - 1.0f/m_angularMotorDecayTimescale);
            }
            else
            {
                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;
                if (Type == Vehicle.TYPE_BOAT)
                {
                    VAservo = 0.2f/(m_verticalAttractionTimescale);
                    VAservo *= (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency);
                }
                else
                {
                    if (parent.LinkSetIsColliding)
                        VAservo = 0.05f/(m_verticalAttractionTimescale);
                    else
                        VAservo = 0.2f/(m_verticalAttractionTimescale);
                    VAservo *= (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency);
                }
                // 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)), 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)
            {
            // 20131224 not used                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);
                if (m_angularMotorVelocity.X == 0)
                {
                    /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
                    {
                        Vector3 axisAngle;
                        float angle;
                        parent.Orientation.GetAxisAngle(out axisAngle, out angle);
                        Vector3 rotatedVel = parent.Velocity * parent.Orientation;
                        if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
                        else
                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
                    }*/
                }
                else
                    banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X)*4;
                if (!parent.LinkSetIsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
                    //If they are colliding, we probably shouldn't shove the prim around... probably
                {
                    float angVelZ = m_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;
                }
                m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
            }

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

            #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

            // Apply to the body

            if (m_lastAngularVelocity.LengthSquared() < 0.0001f)
            {
                m_lastAngularVelocity = Vector3.Zero;
                d.BodySetAngularVel(Body, 0, 0, 0);
                m_angularZeroFlag = true;
            }
            else
            {
                if (!d.BodyIsEnabled(Body))
                    d.BodyEnable(Body);
                d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
                m_angularZeroFlag = false;

                m_lastAngularVelocity.X *= (1.0f - 1.0f/m_angularFrictionTimescale.X);
                m_lastAngularVelocity.Y *= (1.0f - 1.0f/m_angularFrictionTimescale.Y);
                m_lastAngularVelocity.Z *= (1.0f - 1.0f/m_angularFrictionTimescale.Z);
            }
        }
Exemplo n.º 6
0
 //end SetDefaultsForType
 internal void Enable(IntPtr pBody, WhiteCoreODEPrim parent, WhiteCoreODEPhysicsScene pParentScene)
 {
     if (m_enabled)
         return;
     if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
         return;
     m_body = pBody;
     d.BodySetGravityMode(Body, true);
     m_enabled = true;
     m_lastLinearVelocityVector = parent.Velocity;
     m_lastPositionVector = parent.Position;
     m_lastAngularVelocity = parent.RotationalVelocity;
     parent.ThrottleUpdates = false;
     GetMass(pBody);
 }
Exemplo n.º 7
0
        public PhysicsScene GetScene()
        {
            lock (m_lock)
            {
                if (_mScene == null)
                {
                    if (!m_initialized) //Only initialize ode once!
                    {
                        // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
                        // http://opensimulator.org/mantis/view.php?id=2750).
                        d.InitODE();
                        m_initialized = true;
                    }

                    _mScene = new WhiteCoreODEPhysicsScene();
                }
            }

            return _mScene;
        }
Exemplo n.º 8
0
        public WhiteCoreODECharacter(String avName, WhiteCoreODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation,
            Vector3 size)
        {
            m_uuid = UUID.Random();
            _parent_scene = parent_scene;

            m_taintRotation = rotation;

            if (pos.IsFinite())
            {
                if (pos.Z > 9999999f || pos.Z < -90f)
                {
                    pos.Z =
                        _parent_scene.GetTerrainHeightAtXY(_parent_scene.Region.RegionSizeX*0.5f,
                                                           _parent_scene.Region.RegionSizeY*0.5f) + 5.0f;
                }
                _position = pos;
            }
            else
            {
                _position.X = _parent_scene.Region.RegionSizeX*0.5f;
                _position.Y = _parent_scene.Region.RegionSizeY*0.5f;
                _position.Z = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + 10f;

                MainConsole.Instance.Warn("[PHYSICS]: Got NaN Position on Character Create");
            }

            m_isPhysical = false; // current status: no ODE information exists
            Size = size;
            Name = avName;
        }
Exemplo n.º 9
0
 public ODESpecificAvatar(String avName, WhiteCoreODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation,
     Vector3 size)
     : base(avName, parent_scene, pos, rotation, size)
 {
     base._parent_ref = this;
 }
Exemplo n.º 10
0
        public WhiteCoreODEPrim(string name, byte physicsType, PrimitiveBaseShape shape, Vector3 position, Vector3 size, Quaternion rotation, 
            int material, float friction, float restitution, float gravityMultiplier, float density, WhiteCoreODEPhysicsScene parent_scene)
        {
            m_vehicle = new WhiteCoreODEDynamics();

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

            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;

            prim_geom = IntPtr.Zero;

            _name = name;
            PhysicsShapeType = physicsType;
            _size = size;
            _position = position;
            fakepos = 0;
            _orientation = rotation;
            fakeori = 0;
            _pbs = shape;

            _parent_scene = parent_scene;
            m_targetSpace = IntPtr.Zero;

            /*
                        m_isphysical = pisPhysical;
                        if (m_isphysical)
                            m_targetSpace = _parent_scene.space;
            */
            m_isphysical = false;

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

            hasOOBoffsetFromMesh = false;
            _triMeshData = IntPtr.Zero;

            SetMaterial(material, friction, restitution, gravityMultiplier, density);

            CalcPrimBodyData();

            _parent_scene.AddSimulationChange(() => changeadd());
        }