示例#1
0
        private void changeOrientation(Quaternion newOri)
        {
            if (m_orientation != newOri)
            {
                m_orientation = newOri; // keep a copy for core use
                // but only use rotations around Z

                m_orientation2D.W = newOri.W;
                m_orientation2D.Z = newOri.Z;

                float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
                if (t > 0)
                {
                    t = 1.0f / (float)Math.Sqrt(t);
                    m_orientation2D.W *= t;
                    m_orientation2D.Z *= t;
                }
                else
                {
                    m_orientation2D.W = 1.0f;
                    m_orientation2D.Z = 0f;
                }
                m_orientation2D.Y = 0f;
                m_orientation2D.X = 0f;

                d.Quaternion myrot = new d.Quaternion();
                myrot.X = m_orientation2D.X;
                myrot.Y = m_orientation2D.Y;
                myrot.Z = m_orientation2D.Z;
                myrot.W = m_orientation2D.W;
                d.BodySetQuaternion(Body, ref myrot);
            }
        }
示例#2
0
        private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
        {
            if (m_isphysical)
            {
                if (childPrim && m_building)  // inertia is messed, must rebuild
                {
                    _position = newPos;
                    _orientation = newOri;
                }
                else
                {
                    if (newOri != _orientation)
                    {
                        d.Quaternion myrot = new d.Quaternion();
                        myrot.X = newOri.X;
                        myrot.Y = newOri.Y;
                        myrot.Z = newOri.Z;
                        myrot.W = newOri.W;
                        d.GeomSetQuaternion(prim_geom, ref myrot);
                        _orientation = newOri;
                        if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
                            createAMotor(m_angularlock);
                    }
                    if (_position != newPos)
                    {
                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
                        _position = newPos;
                    }
                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
                        d.BodyEnable(Body);
                }
            }
            else
            {
                // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
                // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);

                if (prim_geom != IntPtr.Zero)
                {
                    if (newOri != _orientation)
                    {
                        d.Quaternion myrot = new d.Quaternion();
                        myrot.X = newOri.X;
                        myrot.Y = newOri.Y;
                        myrot.Z = newOri.Z;
                        myrot.W = newOri.W;
                        d.GeomSetQuaternion(prim_geom, ref myrot);
                        _orientation = newOri;
                    }

                    if (newPos != _position)
                    {
                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
                        _position = newPos;

                        m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
                    }
                }
            }
            givefakepos--;
            if (givefakepos < 0)
                givefakepos = 0;
            givefakeori--;
            if (givefakeori < 0)
                givefakeori = 0;

            resetCollisionAccounting();
        }
示例#3
0
        private void changeprimsizeshape()
        {
            _parent_scene.actor_name_map.Remove(prim_geom);
            
            OdePrim parent = (OdePrim)_parent;

            bool chp = childPrim;

            if (chp)
            {
                if (parent != null)
                {
                    parent.DestroyBody();
                }
            }
            else
            {
                DestroyBody();
            }

            if (prim_geom != IntPtr.Zero)
            {
                try
                {
                    d.GeomDestroy(prim_geom);
                    if (_triMeshData != IntPtr.Zero)
                    {
                        d.GeomTriMeshDataDestroy(_triMeshData);
                        _triMeshData = IntPtr.Zero;
                    }
                }
                //                catch (System.AccessViolationException)
                catch (Exception e)
                {
                    m_log.Error("[PHYSICS]: PrimGeom destruction failed: {0}", e);
                }

                prim_geom = IntPtr.Zero;
            }

            // we don't need to do space calculation because the client sends a position update also.
            if (_size.X <= 0)
                _size.X = 0.01f;
            if (_size.Y <= 0)
                _size.Y = 0.01f;
            if (_size.Z <= 0)
                _size.Z = 0.01f;
            // Construction of new prim

            IMesh mesh = null;

            if (_parent_scene.needsMeshing(_pbs))
            {
                mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true);
            }

            CreateGeom(m_targetSpace, mesh);

            if (prim_geom != IntPtr.Zero)
            {
                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                d.Quaternion myrot = new d.Quaternion();
                myrot.X = _orientation.X;
                myrot.Y = _orientation.Y;
                myrot.Z = _orientation.Z;
                myrot.W = _orientation.W;
                d.GeomSetQuaternion(prim_geom, ref myrot);
            }

            if (chp)
            {
                if (parent != null)
                {
                    parent.MakeBody();
                }
            }
            else
                MakeBody();

            resetCollisionAccounting();
        }
示例#4
0
        private void changeadd()
        {
            IMesh mesh = null;
            if (_parent_scene.needsMeshing(_pbs))
            {
                // Don't need to re-enable body..   it's done in SetMesh
                mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true);
                // createmesh returns null when it's a shape that isn't a cube.
                // m_log.Debug(m_localID);
            }

                //Console.WriteLine("changeadd 1");
            CreateGeom(m_targetSpace, mesh);

            if (prim_geom != IntPtr.Zero)
            {
                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                d.Quaternion myrot = new d.Quaternion();
                myrot.X = _orientation.X;
                myrot.Y = _orientation.Y;
                myrot.Z = _orientation.Z;
                myrot.W = _orientation.W;
                d.GeomSetQuaternion(prim_geom, ref myrot);
                // _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
                if (!m_isphysical)
                    SetInStaticSpace(this);
            }

            if (m_isphysical && Body == IntPtr.Zero)
            {
/*
                if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
                {
                    changeShape(_pbs);
                }
                else
                {
 */
                    MakeBody();
//                }
            }
        }
示例#5
0
 private void changeOrientation(Quaternion newOri)
 {
     if (m_isphysical)
     {
         if (childPrim)  // inertia is messed, must rebuild
         {
             if (m_building)
             {
                 _orientation = newOri;
             }
         }
         else
         {
             if (newOri != _orientation)
             {
                 d.Quaternion myrot = new d.Quaternion();
                 myrot.X = newOri.X;
                 myrot.Y = newOri.Y;
                 myrot.Z = newOri.Z;
                 myrot.W = newOri.W;
                 d.GeomSetQuaternion(prim_geom, ref myrot);
                 _orientation = newOri;
                 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
                     createAMotor(m_angularlock);
             }
             if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
                 d.BodyEnable(Body);
         }
     }
     else
     {
         if (prim_geom != IntPtr.Zero)
         {
             if (newOri != _orientation)
             {
                 d.Quaternion myrot = new d.Quaternion();
                 myrot.X = newOri.X;
                 myrot.Y = newOri.Y;
                 myrot.Z = newOri.Z;
                 myrot.W = newOri.W;
                 d.GeomSetQuaternion(prim_geom, ref myrot);
                 _orientation = newOri;
             }
         }
     }
     givefakeori--;
     if (givefakeori < 0)
         givefakeori = 0;
     resetCollisionAccounting();
 }
示例#6
0
        private void MakeBody()
        {
            if (!m_isphysical) // only physical get bodies
                return;

            if (childPrim)  // child prims don't get bodies;
                return;

            if (m_building)
                return;

            if (prim_geom == IntPtr.Zero)
            {
                m_log.Warn("[PHYSICS]: Unable to link the linkset.  Root has no geom yet");
                return;
            }

            if (Body != IntPtr.Zero)
            {
                d.BodyDestroy(Body);
                Body = IntPtr.Zero;
                m_log.Warn("[PHYSICS]: MakeBody called having a body");
            }

            d.Matrix3 mymat = new d.Matrix3();
            d.Quaternion myrot = new d.Quaternion();
            d.Mass objdmass = new d.Mass { };

            Body = d.BodyCreate(_parent_scene.world);

            DMassDup(ref primdMass, out objdmass);

            // rotate inertia
            myrot.X = _orientation.X;
            myrot.Y = _orientation.Y;
            myrot.Z = _orientation.Z;
            myrot.W = _orientation.W;

            d.RfromQ(out mymat, ref myrot);
            d.MassRotate(ref objdmass, ref mymat);

            // set the body rotation and position
            d.BodySetRotation(Body, ref mymat);

            // recompute full object inertia if needed
            if (childrenPrim.Count > 0)
            {
                d.Matrix3 mat = new d.Matrix3();
                d.Quaternion quat = new d.Quaternion();
                d.Mass tmpdmass = new d.Mass { };
                Vector3 rcm;

                rcm.X = _position.X + objdmass.c.X;
                rcm.Y = _position.Y + objdmass.c.Y;
                rcm.Z = _position.Z + objdmass.c.Z;

                lock (childrenPrim)
                {
                    foreach (OdePrim prm in childrenPrim)
                    {
                        if (prm.prim_geom == IntPtr.Zero)
                        {
                            m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it.  No geom yet");
                            continue;
                        }

                        DMassCopy(ref prm.primdMass, ref tmpdmass);

                        // apply prim current rotation to inertia
                        quat.X = prm._orientation.X;
                        quat.Y = prm._orientation.Y;
                        quat.Z = prm._orientation.Z;
                        quat.W = prm._orientation.W;
                        d.RfromQ(out mat, ref quat);
                        d.MassRotate(ref tmpdmass, ref mat);

                        Vector3 ppos = prm._position;
                        ppos.X += tmpdmass.c.X - rcm.X;
                        ppos.Y += tmpdmass.c.Y - rcm.Y;
                        ppos.Z += tmpdmass.c.Z - rcm.Z;

                        // refer inertia to root prim center of mass position
                        d.MassTranslate(ref tmpdmass,
                            ppos.X,
                            ppos.Y,
                            ppos.Z);

                        d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
                        // fix prim colision cats

                        d.GeomClearOffset(prm.prim_geom);
                        d.GeomSetBody(prm.prim_geom, Body);
                        prm.Body = Body;
                        d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
                    }
                }
            }

            d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
            // associate root geom with body
            d.GeomSetBody(prim_geom, Body);

            d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
            d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);

            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
            myrot.W = -myrot.W;
            d.RfromQ(out mymat, ref myrot);
            d.MassRotate(ref objdmass, ref mymat);
            d.BodySetMass(Body, ref objdmass);
            _mass = objdmass.mass;

            m_collisionCategories |= CollisionCategories.Body;
            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

            // disconnect from world gravity so we can apply buoyancy
            d.BodySetGravityMode(Body, false);

            d.BodySetAutoDisableFlag(Body, true);
            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
            //            d.BodySetLinearDampingThreshold(Body, 0.01f);
            //            d.BodySetAngularDampingThreshold(Body, 0.001f);
            d.BodySetDamping(Body, .001f, .0002f);

            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);

            m_interpenetrationcount = 0;
            m_collisionscore = 0;

            m_disabled = false;

            if (m_targetSpace != _parent_scene.ActiveSpace)
            {
                if (m_targetSpace != IntPtr.Zero)
                {
                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
                    if (d.SpaceQuery(m_targetSpace, prim_geom))
                        d.SpaceRemove(m_targetSpace, prim_geom);
                }

                m_targetSpace = _parent_scene.ActiveSpace;
                d.SpaceAdd(m_targetSpace, prim_geom);
            }

            lock (childrenPrim)
            {
                foreach (OdePrim prm in childrenPrim)
                {
                    if (prm.prim_geom == IntPtr.Zero)
                        continue;

                    Vector3 ppos = prm._position;
                    d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position

                    prm.m_collisionCategories |= CollisionCategories.Body;
                    prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
                    d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
                    d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);

                    if (prm.m_targetSpace != _parent_scene.ActiveSpace)
                    {
                        if (prm.m_targetSpace != IntPtr.Zero)
                        {
                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
                            if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
                                d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
                        }
                        prm.m_targetSpace = _parent_scene.ActiveSpace;
                        d.SpaceAdd(m_targetSpace, prm.prim_geom);
                    }

                    prm.m_disabled = false;
                    prm.m_interpenetrationcount = 0;
                    prm.m_collisionscore = 0;
                    _parent_scene.addActivePrim(prm);
                }
            }

            // The body doesn't already have a finite rotation mode set here
            if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
            {
                createAMotor(m_angularlock);
            }

            _parent_scene.addActivePrim(this);
        }
示例#7
0
 private void UpdateDataFromGeom()
 {
     if (prim_geom != IntPtr.Zero)
     {
         d.Vector3 lpos = d.GeomGetPosition(prim_geom);
         _position.X = lpos.X;
         _position.Y = lpos.Y;
         _position.Z = lpos.Z;
         d.Quaternion qtmp = new d.Quaternion { };
         d.GeomCopyQuaternion(prim_geom, out qtmp);
         _orientation.W = qtmp.W;
         _orientation.X = qtmp.X;
         _orientation.Y = qtmp.Y;
         _orientation.Z = qtmp.Z;
     }
 }
示例#8
0
        public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
        {
            // assumes 1m size grid and constante size square regions
            // needs to know about sims around in future

            float[] _heightmap;

            uint regionsizeX = m_regionWidth;
            uint regionsizeY = m_regionHeight;

            // map is rotated
            uint heightmapWidth = regionsizeY + 2;
            uint heightmapHeight = regionsizeX + 2;

            uint heightmapWidthSamples = heightmapWidth + 1;
            uint heightmapHeightSamples = heightmapHeight + 1;

            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];

            const float scale = 1.0f;
            const float offset = 0.0f;
            const float thickness = 10f;
            const int wrap = 0;

 
            float hfmin = float.MaxValue;
            float hfmax = float.MinValue;
            float val;
            uint xx;
            uint yy;

            uint maxXX = regionsizeX - 1;
            uint maxYY = regionsizeY - 1;
            // flipping map adding one margin all around so things don't fall in edges

            uint xt = 0;
            xx = 0;

            for (uint x = 0; x < heightmapWidthSamples; x++)
            {
                if (x > 1 && xx < maxXX)
                    xx++;
                yy = 0;
                for (uint y = 0; y < heightmapHeightSamples; y++)
                {
                    if (y > 1 && y < maxYY)
                        yy += regionsizeX;

                    val = heightMap[yy + xx];
                    if (val < 0.0f)
                        val = 0.0f; // no neg terrain as in chode
                    _heightmap[xt + y] = val;

                    if (hfmin > val)
                        hfmin = val;
                    if (hfmax < val)
                        hfmax = val;
                }
                xt += heightmapHeightSamples;
            }

            lock (OdeLock)
            {
                d.AllocateODEDataForThread(~0U);

                IntPtr GroundGeom = IntPtr.Zero;
                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
                {
                    RegionTerrain.Remove(pOffset);
                    if (GroundGeom != IntPtr.Zero)
                    {
                        actor_name_map.Remove(GroundGeom);
                        d.GeomDestroy(GroundGeom);

                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
                            {
                            TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
                            TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
                            TerrainHeightFieldHeights.Remove(GroundGeom);
                            }
                    }
                }
                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();

                GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);

                d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0,
                                                heightmapHeight, heightmapWidth ,
                                                 (int)heightmapHeightSamples, (int)heightmapWidthSamples, scale,
                                                offset, thickness, wrap);

                d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);

                GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);

                if (GroundGeom != IntPtr.Zero)
                {
                    d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
                    d.GeomSetCollideBits(GroundGeom, 0);

                    PhysicsActor pa = new NullPhysicsActor();
                    pa.Name = "Terrain";
                    pa.PhysicsActorType = (int)ActorTypes.Ground;
                    actor_name_map[GroundGeom] = pa;

//                    geom_name_map[GroundGeom] = "Terrain";

                    d.Quaternion q = new d.Quaternion();
                    q.X = 0.5f;
                    q.Y = 0.5f;
                    q.Z = 0.5f;
                    q.W = 0.5f;

                    d.GeomSetQuaternion(GroundGeom, ref q);
                    d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0.0f);
                    RegionTerrain.Add(pOffset, GroundGeom);
                    TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
                    TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
                }
            }
        }
 } //end MoveAngular
 internal void LimitRotation(float timestep, IntPtr Body)
 {
     d.Quaternion rot = d.BodyGetQuaternion(Body);
     Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
     d.Quaternion m_rot = new d.Quaternion();
     bool changed = false;
     m_rot.X = rotq.X;
     m_rot.Y = rotq.Y;
     m_rot.Z = rotq.Z;
     m_rot.W = rotq.W;
     if (m_RollreferenceFrame != Quaternion.Identity)
     {
         if (rotq.X >= m_RollreferenceFrame.X)
         {
             m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
         }
         if (rotq.Y >= m_RollreferenceFrame.Y)
         {
             m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
         }
         if (rotq.X <= -m_RollreferenceFrame.X)
         {
             m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
         }
         if (rotq.Y <= -m_RollreferenceFrame.Y)
         {
             m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
         }
         changed = true;
     }
     if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
     {
         m_rot.X = 0;
         m_rot.Y = 0;
         changed = true;
     }
     if (changed)
         d.BodySetQuaternion(Body, ref m_rot);
 }