diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 3e2b71cd45..77ea2aff0f 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -179,6 +179,8 @@ namespace OpenSim.Region.Physics.OdePlugin public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; + private float m_primMass = 10.000006836f; // Aluminum g/cm3; + private byte m_shapetype; private byte m_taintshapetype; @@ -538,7 +540,11 @@ namespace OpenSim.Region.Physics.OdePlugin public override float Mass { - get { return CalculateMass(); } + get + { + CalculateMass(); + return m_primMass; + } } public override Vector3 Force @@ -1316,6 +1322,9 @@ namespace OpenSim.Region.Physics.OdePlugin + m_primMass = returnMass; + if (m_primMass > _parent_scene.maximumMassObject) + m_primMass = _parent_scene.maximumMassObject; // Recursively calculate mass bool HasChildPrim = false; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index c4dc7933f5..f739183fcd 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -395,7 +395,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override float Mass { - get { return _mass; } + get { return primMass; } } public override Vector3 Force diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 2b6bc5960e..f5129cb540 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -107,16 +107,17 @@ namespace OdeAPI ConvexClass, GeomTransformClass, TriMeshClass, - HeightfieldClass, + HeightfieldClass, FirstSpaceClass, SimpleSpaceClass = FirstSpaceClass, HashSpaceClass, QuadTreeSpaceClass, LastSpaceClass = QuadTreeSpaceClass, + UbitTerrainClass, FirstUserClass, LastUserClass = FirstUserClass + MaxUserClasses - 1, NumClasses, - MaxUserClasses = 4 + MaxUserClasses = 5 } public enum JointType : int @@ -201,8 +202,11 @@ namespace OdeAPI [UnmanagedFunctionPointer(CallingConvention.Cdecl)] public delegate void GeomDtorFn(IntPtr o); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal UbitTerrainGetHeight(IntPtr p_user_data, int x, int z); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); @@ -729,6 +733,18 @@ namespace OdeAPI return CreateiHeightfield(space, data, bPlaceable); } + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateUbitTerrain"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiUbitTerrain(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateUbitTerrain(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiUbitTerrain(space, data, bPlaceable); + } + + + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] public static extern IntPtr CreateiGeom(int classnum); public static IntPtr CreateGeom(int classnum) @@ -964,6 +980,8 @@ namespace OdeAPI dReal width, dReal depth, int widthSamples, int depthSamples, dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, dReal width, dReal depth, int widthSamples, int depthSamples, @@ -989,6 +1007,33 @@ namespace OdeAPI [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomUbitTerrainDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomUbitTerrainGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainSetHeightfieldData(IntPtr g, IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] public static extern bool GeomIsEnabled(IntPtr geom); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 1a6907d37d..3e0ccefe65 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -156,6 +156,7 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly ILog m_log; // private Dictionary m_storedCollisions = new Dictionary(); + public bool OdeUbitLib = false; // private int threadid = 0; private Random fluidRandomizer = new Random(Environment.TickCount); @@ -374,7 +375,14 @@ namespace OpenSim.Region.Physics.OdePlugin mesher = meshmerizer; m_config = config; -// m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE")); + string ode_config = d.GetConfiguration("ODE"); + m_log.WarnFormat("ODE configuration: {0}", ode_config); + + if (ode_config.Contains("ODE_Ubit")) + { + OdeUbitLib = true; + } + /* if (region != null) { @@ -527,13 +535,24 @@ namespace OpenSim.Region.Physics.OdePlugin // sets a global contact for a joint for contactgeom , and base contact description) - private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce,float cfm,float erp) + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale) { if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) return IntPtr.Zero; + float erp = contactGeom.depth; + erp *= erpscale; + if (erp < minERP) + erp = minERP; + else if (erp > MaxERP) + erp = MaxERP; + + float depth = contactGeom.depth * dscale; + if (depth > 0.5f) + depth = 0.5f; + d.Contact newcontact = new d.Contact(); - newcontact.geom.depth = contactGeom.depth; + newcontact.geom.depth = depth; newcontact.geom.g1 = contactGeom.g1; newcontact.geom.g2 = contactGeom.g2; newcontact.geom.pos = contactGeom.pos; @@ -692,6 +711,10 @@ namespace OpenSim.Region.Physics.OdePlugin float bounce = 0; float cfm = 0.0001f; float erp = 0.1f; + float erpscale = 1.0f; + float dscale = 1.0f; + bool IgnoreNegSides = false; + ContactData contactdata1 = new ContactData(0, 0, false); ContactData contactdata2 = new ContactData(0, 0, false); @@ -781,10 +804,14 @@ namespace OpenSim.Region.Physics.OdePlugin cfm = p1.Mass; if (cfm > p2.Mass) cfm = p2.Mass; - cfm = (float)Math.Sqrt(cfm); - cfm *= 0.0001f; - if (cfm > 0.8f) - cfm = 0.8f; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + if (dscale > 1.0f) + dscale = 1.0f; + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; @@ -801,11 +828,22 @@ namespace OpenSim.Region.Physics.OdePlugin if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) mu *= frictionMovementMult; p1.CollidingGround = true; + cfm = p1.Mass; - cfm = (float)Math.Sqrt(cfm); - cfm *= 0.0001f; - if (cfm > 0.8f) - cfm = 0.8f; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + if (dscale > 1.0f) + dscale = 1.0f; + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + + if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) + { + if (curContact.side1 > 0) + IgnoreNegSides = true; + } } else if (name == "Water") @@ -830,11 +868,21 @@ namespace OpenSim.Region.Physics.OdePlugin p2.getContactData(ref contactdata2); bounce = contactdata2.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + cfm = p2.Mass; - cfm = (float)Math.Sqrt(cfm); - cfm *= 0.0001f; - if (cfm > 0.8f) - cfm = 0.8f; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + + if (dscale > 1.0f) + dscale = 1.0f; + + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + + if (curContact.side1 > 0) // should be 2 ? + IgnoreNegSides = true; if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) mu *= frictionMovementMult; @@ -862,39 +910,45 @@ namespace OpenSim.Region.Physics.OdePlugin int i = 0; while(true) { - if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) - p1.IsColliding = true; - if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) - p2.IsColliding = true; - - erp = curContact.depth; - if (erp < minERP) - erp = minERP; - else if (erp > MaxERP) - erp = MaxERP; - - Joint = CreateContacJoint(ref curContact, mu, bounce,cfm,erp); - d.JointAttach(Joint, b1, b2); - - if (++m_global_contactcount >= maxContactsbeforedeath) - break; - - if(++i >= count) - break; - - if (!GetCurContactGeom(i, ref curContact)) - break; - - if (curContact.depth > maxDepthContact.PenetrationDepth) + if (IgnoreNegSides && curContact.side1 < 0) { - maxDepthContact.Position.X = curContact.pos.X; - maxDepthContact.Position.Y = curContact.pos.Y; - maxDepthContact.Position.Z = curContact.pos.Z; - maxDepthContact.SurfaceNormal.X = curContact.normal.X; - maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; - maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; - maxDepthContact.PenetrationDepth = curContact.depth; + if (++i >= count) + break; + + if (!GetCurContactGeom(i, ref curContact)) + break; + } + else + + { + if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) + p1.IsColliding = true; + if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) + p2.IsColliding = true; + + Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); + d.JointAttach(Joint, b1, b2); + + if (++m_global_contactcount >= maxContactsbeforedeath) + break; + + if (++i >= count) + break; + + if (!GetCurContactGeom(i, ref curContact)) + break; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.SurfaceNormal.X = curContact.normal.X; + maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; + maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; + maxDepthContact.PenetrationDepth = curContact.depth; + } } } @@ -1865,13 +1919,12 @@ namespace OpenSim.Region.Physics.OdePlugin public float GetTerrainHeightAtXY(float x, float y) { - // assumes 1m size grid and constante size square regions - // needs to know about sims around in future - // region offset in mega position + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + IntPtr heightFieldGeom = IntPtr.Zero; // get region map @@ -1903,28 +1956,55 @@ namespace OpenSim.Region.Physics.OdePlugin int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples - // we still have square fixed size regions - // also flip x and y because of how map is done for ODE fliped axis - // so ix,iy,dx and dy are inter exchanged - if (x < regsize - 1) + if (OdeUbitLib) { - iy = (int)x; - dy = x - (float)iy; - } - else // out world use external height - { - iy = regsize - 1; - dy = 0; - } - if (y < regsize - 1) - { - ix = (int)y; - dx = y - (float)ix; + if (x < regsize - 1) + { + ix = (int)x; + dx = x - (float)ix; + } + else // out world use external height + { + ix = regsize - 1; + dx = 0; + } + if (y < regsize - 1) + { + iy = (int)y; + dy = y - (float)iy; + } + else + { + iy = regsize - 1; + dy = 0; + } } + else { - ix = regsize - 1; - dx = 0; + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsize - 1; + dy = 0; + } + if (y < regsize - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsize - 1; + dx = 0; + } } float h0; @@ -1951,6 +2031,8 @@ namespace OpenSim.Region.Physics.OdePlugin return h0 + h1 + h2; } + + public override void SetTerrain(float[] heightMap) { if (m_worldOffset != Vector3.Zero && m_parentScene != null) @@ -1972,7 +2054,15 @@ namespace OpenSim.Region.Physics.OdePlugin } public void SetTerrain(float[] heightMap, Vector3 pOffset) - { + { + if (OdeUbitLib) + UbitSetTerrain(heightMap, pOffset); + else + OriSetTerrain(heightMap, pOffset); + } + + public void OriSetTerrain(float[] heightMap, Vector3 pOffset) + { // assumes 1m size grid and constante size square regions // needs to know about sims around in future @@ -2086,6 +2176,108 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public void UbitSetTerrain(float[] heightMap, Vector3 pOffset) + { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future + + float[] _heightmap; + + uint heightmapWidth = Constants.RegionSize + 2; + uint heightmapHeight = Constants.RegionSize + 2; + + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; + + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; + + + uint regionsize = Constants.RegionSize; + + float hfmin = float.MaxValue; +// float hfmax = float.MinValue; + float val; + + + uint maxXXYY = regionsize - 1; + // adding one margin all around so things don't fall in edges + + uint xx; + uint yy = 0; + uint yt = 0; + + for (uint y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxXXYY) + yy += regionsize; + xx = 0; + for (uint x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && x < maxXXYY) + xx++; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode + _heightmap[yt + x] = val; + + if (hfmin > val) + hfmin = val; +// if (hfmax < val) +// hfmax = val; + } + yt += heightmapWidthSamples; + } + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + TerrainHeightFieldHeights.Remove(GroundGeom); + } + d.SpaceRemove(StaticSpace, GroundGeom); + d.GeomDestroy(GroundGeom); + } + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + + const int wrap = 0; + float thickness = hfmin; + if (thickness < 0) + thickness = 1; + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, + thickness, wrap); + +// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[GroundGeom] = "Terrain"; + + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); + RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); + // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + } + } + + public override void DeleteTerrain() { }