Exemplo n.º 1
0
        private void MakeBody()
        {
            //            d.Vector3 dvtmp;
            //            d.Vector3 dbtmp;

            if (m_blockPhysicalReconstruction) // building is blocked
                return;

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

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

            if (!m_isphysical) // only physical things get a body
                return;

            if (Body != IntPtr.Zero) // who shouldn't have one already ?
            {
                d.BodyDestroy(Body);
                Body = IntPtr.Zero;
                MainConsole.Instance.Warn("[PHYSICS]: MakeBody called having a body");
            }

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

            Body = d.BodyCreate(_parent_scene.world);
            d.BodySetData(Body, (IntPtr)ActorTypes.Prim);

            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 (AuroraODEPrim prm in childrenPrim)
                    {
                        if (prm.prim_geom == IntPtr.Zero)
                        {
                            MainConsole.Instance.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.W = prm._orientation.W;
                        quat.X = prm._orientation.X;
                        quat.Y = prm._orientation.Y;
                        quat.Z = prm._orientation.Z;
                        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
            //            if (!testRealGravity)
            d.BodySetGravityMode(Body, false);

            d.BodySetAutoDisableFlag(Body, true);
            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
            d.BodySetDamping(Body, .001f, .0002f);
            m_disabled = false;

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

            if (m_targetSpace != _parent_scene.space)
            {
                if (d.SpaceQuery(m_targetSpace, prim_geom))
                    d.SpaceRemove(m_targetSpace, prim_geom);

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

            lock (childrenPrim)
            {
                foreach (AuroraODEPrim 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.space)
                    {
                        if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
                            d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);

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

                    prm.m_disabled = false;
                    _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);
            }
            if (m_vehicle.Type != Vehicle.TYPE_NONE)
                m_vehicle.Enable(Body, this, _parent_scene);

            _parent_scene.addActivePrim(this);
        }
        public override void SetTerrain(ITerrainChannel channel, short[] heightMap)
        {
            m_channel = channel;
            float[] _heightmap = ODETerrainHeightFieldHeights;
            if (ODETerrainHeightFieldHeights == null)
                _heightmap = new float[m_region.RegionSizeX*m_region.RegionSizeY];

            for (int x = 0; x < m_region.RegionSizeX; x++)
            {
                for (int y = 0; y < m_region.RegionSizeY; y++)
                {
                    _heightmap[(x*m_region.RegionSizeX) + y] = heightMap[y*m_region.RegionSizeX + x]/
                                                               Constants.TerrainCompression;
                }
            }

            float hfmin = _heightmap.Min();
            float hfmax = _heightmap.Max();

            SimulationChangesQueue.Enqueue(() =>
                                               {
                                                   if (RegionTerrain != IntPtr.Zero)
                                                   {
                                                       d.GeomHeightfieldDataDestroy(RegionTerrain);
                                                       d.SpaceRemove(space, RegionTerrain);
                                                       //d.GeomDestroy(RegionTerrain);
                                                       GC.RemoveMemoryPressure(_heightmap.Length);
                                                   }

                                                   const float scale = 1f;
                                                   const float offset = 0.0f;
                                                   float thickness = 0.2f;
                                                   const int wrap = 0;

                                                   IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
                                                   GC.AddMemoryPressure(_heightmap.Length);
                                                   //Add the memory pressure properly (note: should we be doing this since we have it in managed memory?)
                                                   //Do NOT copy it! Otherwise, it'll copy the terrain into unmanaged memory where we can't release it each time
                                                   d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
                                                                                    m_region.RegionSizeX,
                                                                                    m_region.RegionSizeY,
                                                                                    m_region.RegionSizeX,
                                                                                    m_region.RegionSizeY, scale,
                                                                                    offset, thickness, wrap);

                                                   d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1.0f,
                                                                                  hfmax + 1.0f);
                                                   RegionTerrain = d.CreateHeightfield(space, HeightmapData, 1);

                                                   d.GeomSetCategoryBits(RegionTerrain, (int) (CollisionCategories.Land));
                                                   d.GeomSetCollideBits(RegionTerrain, (int) (CollisionCategories.Space));

                                                   actor_name_map[RegionTerrain] = new NullObjectPhysicsActor();

                                                   d.Matrix3 R = new d.Matrix3();

                                                   Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0),
                                                                                                  1.5707f);
                                                   Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0),
                                                                                                  1.5707f);

                                                   q1 = q1*q2;

                                                   Vector3 v3;
                                                   float angle;
                                                   q1.GetAxisAngle(out v3, out angle);

                                                   d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);

                                                   d.GeomSetRotation(RegionTerrain, ref R);
                                                   d.GeomSetPosition(RegionTerrain, (m_region.RegionSizeX*0.5f),
                                                                     (m_region.RegionSizeY*0.5f), 0);

                                                   TerrainHeightFieldHeights = heightMap;
                                                   ODETerrainHeightFieldHeights = _heightmap;
                                               });
        }