C# AuroraOpenDynamicsEngine.AuroraODEPhysicsScene类代码示例

 public PhysicsScene GetScene(String sceneIdentifier)
 {
     if (_mScene == null)
     {
         // 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();
         
         _mScene = new AuroraODEPhysicsScene(ode, sceneIdentifier);
     }
     return _mScene;
 }

        public PhysicsScene GetScene(String sceneIdentifier)
        {
            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.SetODEFile();
                    d.InitODE();
                    m_initialized = true;
                }

                _mScene = new AuroraODEPhysicsScene(sceneIdentifier);
            }
            return _mScene;
        }

        public bool DoAChange(AuroraODEPhysicsScene.changes what, object arg)
        {
            if (m_frozen && what != AuroraODEPhysicsScene.changes.Add && what != AuroraODEPhysicsScene.changes.Remove)
                return false;

            if (prim_geom == IntPtr.Zero && what != AuroraODEPhysicsScene.changes.Add &&
                what != AuroraODEPhysicsScene.changes.Remove)
            {
                m_frozen = true;
                return false;
            }

            // nasty switch
            switch (what)
            {
                case AuroraODEPhysicsScene.changes.Add:
                    changeadd();
                    break;
                case AuroraODEPhysicsScene.changes.Remove:
                    if (_parent != null)
                    {
                        AuroraODEPrim parent = (AuroraODEPrim)_parent;
                        parent.ChildRemove(this);
                    }
                    else
                        ChildRemove(this);

                    RemoveGeom();
                    m_targetSpace = IntPtr.Zero;
                    return true;

                case AuroraODEPhysicsScene.changes.Link:
                    AuroraODEPrim tmp = (AuroraODEPrim)arg;
                    changelink(tmp);
                    break;

                case AuroraODEPhysicsScene.changes.DeLink:
                    changelink(null);
                    break;

                case AuroraODEPhysicsScene.changes.Position:
                    changePosition((Vector3)arg);
                    break;

                case AuroraODEPhysicsScene.changes.Orientation:
                    changeOrientation((Quaternion)arg);
                    break;

                case AuroraODEPhysicsScene.changes.PosOffset:
                    donullchange();
                    break;

                case AuroraODEPhysicsScene.changes.OriOffset:
                    donullchange();
                    break;

                case AuroraODEPhysicsScene.changes.Velocity:
                    changevelocity(arg);
                    break;

                case AuroraODEPhysicsScene.changes.Acceleration:
                    changeacceleration(arg);
                    break;
                case AuroraODEPhysicsScene.changes.AngVelocity:
                    changeangvelocity(arg);
                    break;

                case AuroraODEPhysicsScene.changes.Force:
                    changeforce(arg);
                    break;

                case AuroraODEPhysicsScene.changes.Torque:
                    changeSetTorque(arg);
                    break;

                case AuroraODEPhysicsScene.changes.AddForce:
                    changeAddForce(arg);
                    break;

                case AuroraODEPhysicsScene.changes.AddAngForce:
                    changeAddAngularForce(arg);
                    break;

                case AuroraODEPhysicsScene.changes.AngLock:
                    changeAngularLock(arg);
                    break;

                case AuroraODEPhysicsScene.changes.Size:
                    changesize(arg);
                    break;

                case AuroraODEPhysicsScene.changes.Shape:
                    changeshape(arg);
                    break;

                case AuroraODEPhysicsScene.changes.CollidesWater:
                    changefloatonwater(arg);
                    break;

                case AuroraODEPhysicsScene.changes.VolumeDtc:
//.........这里部分代码省略.........

        public AuroraODEPrim(String primName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Vector3 size,
                       Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, float Density)
        {
            m_vehicle = new AuroraODEDynamics();
            //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
            ode = dode;
            if (!pos.IsFinite())
            {
                pos = new Vector3((parent_scene.Region.RegionSizeX * 0.5f), (parent_scene.Region.RegionSizeY * 0.5f),
                    parent_scene.GetTerrainHeightAtXY((parent_scene.Region.RegionSizeX * 0.5f), (parent_scene.Region.RegionSizeY * 0.5f)));
                m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
            }
            _position = pos;
            fakepos = false;

            PID_D = parent_scene.bodyPIDD;
            PID_G = parent_scene.bodyPIDG;

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

            m_density = Density / 100;
            // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;


            prim_geom = IntPtr.Zero;
            prev_geom = IntPtr.Zero;

            if (!size.IsFinite())
            {
                size = new Vector3(0.5f, 0.5f, 0.5f);
                m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
            }

            if (size.X <= 0) size.X = 0.01f;
            if (size.Y <= 0) size.Y = 0.01f;
            if (size.Z <= 0) size.Z = 0.01f;

            _size = size;

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

            _orientation = rotation;
            fakeori = false;

            _mesh = mesh;
            _pbs = pbs;

            _parent_scene = parent_scene;
            m_targetSpace = (IntPtr)0;

            if (pos.Z < 0)
                m_isphysical = false;
            else
            {
                m_isphysical = pisPhysical;
                // If we're physical, we need to be in the master space for now.
                // linksets *should* be in a space together..  but are not currently
                if (m_isphysical)
                    m_targetSpace = _parent_scene.space;
            }
            m_primName = primName;

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

            m_UpdateTimecntr = 0;
            m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
            if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
                m_UpdateTimecntr = .1f;

            AddChange(changes.Add, null);
        }

 public AuroraODERayCastRequestManager(AuroraODEPhysicsScene pScene)
 {
     m_scene = pScene;
     nearCallback = near;
 }

        public AuroraODEPrim(string name, byte physicsType, PrimitiveBaseShape shape, Vector3 position, Vector3 size, Quaternion rotation, 
            int material, float friction, float restitution, float gravityMultiplier, float density, AuroraODEPhysicsScene parent_scene)
        {
            m_vehicle = new AuroraODEDynamics();

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

        public AuroraODECharacter(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation, Vector3 size)
        {
            m_uuid = UUID.Random();

            m_taintRotation = rotation;
//            _orientation = rotation;
//            _lastorientation = rotation;

            if (pos.IsFinite())
            {
                if (pos.Z > 9999999f || pos.Z <-90f)
                {
//                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                    pos.Z = parent_scene.GetTerrainHeightAtXY(parent_scene.Region.RegionSizeX * 0.5f, parent_scene.Region.RegionSizeY * 0.5f) + 5.0f;
                }
                _position = pos;
                m_taintPosition.X = pos.X;
                m_taintPosition.Y = pos.Y;
                m_taintPosition.Z = pos.Z;
            }
            else
            {
                _position.X = (float)parent_scene.Region.RegionSizeX * 0.5f;
                _position.Y = (float)parent_scene.Region.RegionSizeY * 0.5f;
                _position.Z = parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + 10f;
                    
                m_taintPosition.X = _position.X;
                m_taintPosition.Y = _position.Y;
                m_taintPosition.Z = _position.Z;
                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
            }

            _parent_scene = parent_scene;

            CAPSULE_RADIUS = parent_scene.avCapRadius;

            // m_StandUpRotation =
            //     new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
            //                   0.5f);

            CAPSULE_LENGTH = (size.Z * 1.1f) - CAPSULE_RADIUS * 2.0f;

            if ((m_collisionFlags & CollisionCategories.Land) == 0)
                AvatarHalfsize = CAPSULE_LENGTH * 0.5f + CAPSULE_RADIUS;
            else
                AvatarHalfsize = CAPSULE_LENGTH * 0.5f + CAPSULE_RADIUS - 0.3f;

            //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
            m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;

            m_isPhysical = false; // current status: no ODE information exists
            m_tainted_isPhysical = true; // new tainted status: need to create ODE information

            _parent_scene.AddPhysicsActorTaint(this);

            m_UpdateTimecntr = 0;
            m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
            if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
                m_UpdateTimecntr = .1f;
            m_name = avName;
        }

        public AuroraODECharacter(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation,
                                  Vector3 size)
        {
            m_uuid = UUID.Random();
            _parent_scene = parent_scene;

            m_taintRotation = rotation;

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

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


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

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

        // end Step

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

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

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

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

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

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

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

            #region Blocking Change

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

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

            #endregion

            #region Terrain checks

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

            #endregion

            #region Hover

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

        internal void Step(IntPtr pBody, float pTimestep, AuroraODEPhysicsScene pParentScene, AuroraODEPrim 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);
        }

//end SetDefaultsForType

        internal void Enable(IntPtr pBody, AuroraODEPrim parent, AuroraODEPhysicsScene pParentScene)
        {
            if (m_enabled)
                return;
            m_enabled = true;
            m_lastLinearVelocityVector = parent.Velocity;
            m_lastPositionVector = parent.Position;
            m_lastAngularVelocity = parent.RotationalVelocity;
            parent.ThrottleUpdates = false;
            m_body = pBody;
            if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
                return;
            GetMass(pBody);
        }

        public AuroraODECharacter(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation,
                                  Vector3 size)
        {
            m_uuid = UUID.Random();
            _parent_scene = parent_scene;

            m_taintRotation = rotation;

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

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


            m_isPhysical = false; // current status: no ODE information exists
            Size = size;
            m_name = avName;
        }

        internal void Step(IntPtr pBody, float pTimestep, AuroraODEPhysicsScene pParentScene, AuroraODEPrim parent)
        {
            m_body = pBody;
            if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
                return;
            if (!d.BodyIsEnabled(Body))
                d.BodyEnable(Body);

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

            MoveLinear(pTimestep, pParentScene);
            MoveAngular(pTimestep, pParentScene);
            LimitRotation(pTimestep);

            /*if (!parent.m_angularlock.ApproxEquals(Vector3.One, 0.003f) &&
                                parent.Amotor != IntPtr.Zero)
            {
                d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
                d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 9000f);
                d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
                d.JointSetAMotorParam(Amotor, (int)dParam.FMax, int.MaxValue);
                d.Vector3 avel2 = d.BodyGetAngularVel(Body);

                if (parent.m_angularlock.X == 0)
                    avel2.X = 0;
                if (parent.m_angularlock.Y == 0)
                    avel2.Y = 0;
                if (parent.m_angularlock.Z == 0)
                    avel2.Z = 0;
                d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
                d.BodySetAngularDamping(Body, 1);
                d.BodySetTorque(Body, 0, 0, 0);
            }*/

            // WE deal with updates
            parent.RequestPhysicsterseUpdate();
        }   // end Step

 public void AddChange(AuroraODEPhysicsScene.changes what, object arg)
 {
     _parent_scene.AddChange(this, what, arg);
 }

        private Vector3 showposition; // a temp hack for now rest of code expects position to be changed immediately

        public AuroraODEPrim(ISceneChildEntity entity, AuroraODEPhysicsScene parent_scene, bool pisPhysical)
        {
            m_vehicle = new AuroraODEDynamics();
            //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);

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

            _size = entity.Scale;
            _position = entity.AbsolutePosition;
            fakepos = 0;
            _orientation = entity.GetWorldRotation();
            fakeori = 0;
            _pbs = entity.Shape;
            _parent_entity = entity;

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

            CalcPrimBodyData();

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