diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index e8894f701a..67f8af5a3e 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -180,6 +180,7 @@ namespace OpenSim.Region.Physics.OdePlugin internal int m_material = (int)Material.Wood; private int frcount = 0; // Used to limit dynamics debug output to + private int revcount = 0; // Reverse motion while > 0 private IntPtr m_body = IntPtr.Zero; @@ -1888,22 +1889,19 @@ namespace OpenSim.Region.Physics.OdePlugin public void changemove(float timestep) { -//Console.WriteLine("changemove for {0}", m_primName ); - +//Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position ); if (m_isphysical) { //Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim); // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! if (!m_taintremove && !childPrim) { -//Console.WriteLine("physOK"); if (Body == IntPtr.Zero) enableBody(); //Prim auto disable after 20 frames, //if you move it, re-enable the prim manually. if (_parent != null) { -//Console.WriteLine("physChild"); if (m_linkJoint != IntPtr.Zero) { d.JointDestroy(m_linkJoint); @@ -1912,7 +1910,6 @@ namespace OpenSim.Region.Physics.OdePlugin } if (Body != IntPtr.Zero) { -//Console.WriteLine("physNotIPZ"); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); if (_parent != null) @@ -1945,7 +1942,6 @@ Console.WriteLine(" JointCreateFixed"); } else { -//Console.WriteLine("NONphys"); // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); _parent_scene.waitForSpaceUnlock(m_targetSpace); @@ -2382,9 +2378,7 @@ Console.WriteLine(" JointCreateFixed"); if (IsPhysical) { if (Body != IntPtr.Zero) - { d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); - } } //resetCollisionAccounting(); @@ -2394,31 +2388,9 @@ Console.WriteLine(" JointCreateFixed"); public void UpdatePositionAndVelocity() { - return; // moved to the Move() method + return; // moved to the Move () method } -/* No one uses this? - public Matrix4 FromDMass(d.Mass pMass) - { - Matrix4 obj; - obj.M11 = pMass.I.M00; - obj.M12 = pMass.I.M01; - obj.M13 = pMass.I.M02; - obj.M14 = 0; - obj.M21 = pMass.I.M10; - obj.M22 = pMass.I.M11; - obj.M23 = pMass.I.M12; - obj.M24 = 0; - obj.M31 = pMass.I.M20; - obj.M32 = pMass.I.M21; - obj.M33 = pMass.I.M22; - obj.M34 = 0; - obj.M41 = 0; - obj.M42 = 0; - obj.M43 = 0; - obj.M44 = 1; - return obj; - } -*/ + public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) { obj.I.M00 = pMat[0, 0]; @@ -2996,44 +2968,114 @@ Console.WriteLine(" JointCreateFixed"); float fx = 0; float fy = 0; float fz = 0; + Vector3 linvel; // velocity applied, including any reversal + int outside = 0; + + // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. + // This is a temp patch until proper region crossing is developed. + + int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; + int fence = _parent_scene.geomRegionFence; + + float border_limit = 0.05f; // original limit + if (fence == 1) border_limit = 0.5f; // bounce point frcount++; // used to limit debug comment output if (frcount > 100) frcount = 0; - if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. + if(revcount > 0) revcount--; + + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims. { - -// Old public void UpdatePositionAndVelocity(), more accuratley calculated here + // Old public void UpdatePositionAndVelocity(), more accuratley calculated here bool lastZeroFlag = _zeroFlag; // was it stopped + d.Vector3 vec = d.BodyGetPosition(Body); - d.Quaternion ori = d.BodyGetQuaternion(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - // d.Vector3 rotvel = d.BodyGetAngularVel(Body); - d.Vector3 torque = d.BodyGetTorque(Body); - _torque = new Vector3(torque.X, torque.Y, torque.Z); Vector3 l_position = Vector3.Zero; - Quaternion l_orientation = Quaternion.Identity; - - m_lastposition = _position; - m_lastorientation = _orientation; - l_position.X = vec.X; l_position.Y = vec.Y; l_position.Z = vec.Z; - l_orientation.X = ori.X; - l_orientation.Y = ori.Y; - l_orientation.Z = ori.Z; - l_orientation.W = ori.W; + m_lastposition = _position; + _position = l_position; + + d.Quaternion ori = d.BodyGetQuaternion(Body); + // Quaternion l_orientation = Quaternion.Identity; + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + m_lastorientation = _orientation; + + d.Vector3 vel = d.BodyGetLinearVel(Body); + m_lastVelocity = _velocity; + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + _acceleration = ((_velocity - m_lastVelocity) / timestep); + + d.Vector3 torque = d.BodyGetTorque(Body); + _torque = new Vector3(torque.X, torque.Y, torque.Z); + + base.RequestPhysicsterseUpdate(); + //Console.WriteLine("Move {0} at {1}", m_primName, l_position); - // Check if outside region horizontally - if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || - l_position.X < 0f || - l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || - l_position.Y < 0f) + // Check if outside region + // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! + if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit)) { - if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) + l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit); + outside = 1; + } + + if (l_position.X < border_limit) + { + l_position.X = border_limit; + outside = 2; + } + if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit)) + { + l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit); + outside = 3; + } + + if (l_position.Y < border_limit) + { + l_position.Y = border_limit; + outside = 4; + } + + if (outside > 0) + { +//Console.WriteLine(" fence = {0}",fence); + +//Console.WriteLine("Border {0}", l_position); + if (fence == 1) // bounce object off boundary + { + if (revcount == 0) + { + if (outside < 3) + { + _velocity.X = -_velocity.X; + } + else + { + _velocity.Y = -_velocity.Y; + } + if (m_type != Vehicle.TYPE_NONE) Halt(); + _position = l_position; + m_taintposition = _position; + m_lastVelocity = _velocity; + _acceleration = Vector3.Zero; + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + base.RequestPhysicsterseUpdate(); + + revcount = 25; // wait for object to move away from border + } + } // else old crossing mode + else if (m_crossingfailures < failureLimit) { // keep trying to cross? _position = l_position; //_parent_scene.remActivePrim(this); @@ -3043,9 +3085,12 @@ Console.WriteLine(" JointCreateFixed"); else { // Too many tries if (_parent == null) base.RaiseOutOfBounds(l_position); +//Console.WriteLine("ROOB 2"); + return; // Dont process any other motion? - } + } // end various methods } // end outside region horizontally + if (l_position.Z < 0) { @@ -3057,6 +3102,8 @@ Console.WriteLine(" JointCreateFixed"); //IsPhysical = false; if (_parent == null) base.RaiseOutOfBounds(_position); +//Console.WriteLine("ROOB 3"); + _acceleration.X = 0; // This stuff may stop client display but it has no _acceleration.Y = 0; // effect on the object in phys engine! @@ -3081,10 +3128,9 @@ Console.WriteLine(" JointCreateFixed"); if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) - && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large + && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // KF 0.01 is far to large { _zeroFlag = true; -//Console.WriteLine("ZFT 2"); m_throttleUpdates = false; } else @@ -3104,10 +3150,7 @@ Console.WriteLine(" JointCreateFixed"); _acceleration.X = 0; _acceleration.Y = 0; _acceleration.Z = 0; - //_orientation.w = 0f; - //_orientation.X = 0f; - //_orientation.Y = 0f; - //_orientation.Z = 0f; + m_rotationalVelocity.X = 0; m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; @@ -3132,26 +3175,6 @@ Console.WriteLine(" JointCreateFixed"); base.RequestPhysicsterseUpdate(); } } - - m_lastVelocity = _velocity; - - _position = l_position; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; -// Why 2 calcs??? -// _acceleration = ((_velocity - m_lastVelocity) / 0.1f); -// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, -// _velocity.Y - m_lastVelocity.Y / 0.1f, -// _velocity.Z - m_lastVelocity.Z / 0.1f); - - _acceleration = ((_velocity - m_lastVelocity) / timestep); - - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; m_lastUpdateSent = false; if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) { @@ -3167,11 +3190,8 @@ Console.WriteLine(" JointCreateFixed"); } m_lastposition = l_position; - /// End of old UpdatePositionAndVelocity insert + /// End UpdatePositionAndVelocity insert -//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration); -// if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type + -// " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); if (m_type != Vehicle.TYPE_NONE) { // get body attitude @@ -3299,11 +3319,12 @@ Console.WriteLine(" JointCreateFixed"); { // not hovering, Gravity rules m_wLinObjectVel.Z = vel_now.Z; } - - + linvel = m_wLinObjectVel; + // Vehicle Linear Motion done ======================================= // Apply velocity - d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z); +//if(frcount == 0) Console.WriteLine("LV {0}", linvel); + d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); // apply gravity force d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); //if(frcount == 0) Console.WriteLine("Grav {0}", grav); @@ -3626,9 +3647,10 @@ Console.WriteLine(" JointCreateFixed"); // react to the physics scene by moving it's position. // Avatar to Avatar collisions // Prim to avatar collisions + d.Vector3 dlinvel = vel; d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); d.BodyAddForce(Body, 0, 0, fz); //KF this prevents furthur motions return; } diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index deb61641e2..79e2986231 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -224,6 +224,7 @@ namespace OpenSim.Region.Physics.OdePlugin public float bodyPIDG = 25; public int geomCrossingFailuresBeforeOutofbounds = 5; + public int geomRegionFence = 0; public float bodyMotorJointMaxforceTensor = 2; @@ -447,6 +448,7 @@ namespace OpenSim.Region.Physics.OdePlugin geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); + geomRegionFence = physicsconfig.GetInt("region_border_fence", 0); geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); @@ -3419,76 +3421,56 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetTerrain(float[] heightMap, Vector3 pOffset) { - // this._heightmap[i] = (double)heightMap[i]; - // dbm (danx0r) -- creating a buffer zone of one extra sample all around - //_origheightmap = heightMap; + uint regionsize = (uint) Constants.RegionSize; // visible region size eg. 256(M) + + uint heightmapWidth = regionsize + 1; // ODE map size 257 x 257 (Meters) (1 extra + uint heightmapHeight = regionsize + 1; + + uint heightmapWidthSamples = (uint)regionsize + 2; // Sample file size, 258 x 258 samples + uint heightmapHeightSamples = (uint)regionsize + 2; + + // Array of height samples for ODE float[] _heightmap; + _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258 - // zero out a heightmap array float array (single dimension [flattened])) - //if ((int)Constants.RegionSize == 256) - // _heightmap = new float[514 * 514]; - //else - - _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; - - uint heightmapWidth = Constants.RegionSize + 1; - uint heightmapHeight = Constants.RegionSize + 1; - - uint heightmapWidthSamples; - - uint heightmapHeightSamples; - - //if (((int)Constants.RegionSize) == 256) - //{ - // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2; - // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2; - // heightmapWidth++; - // heightmapHeight++; - //} - //else - //{ - - heightmapWidthSamples = (uint)Constants.RegionSize + 1; - heightmapHeightSamples = (uint)Constants.RegionSize + 1; - //} - + // Other ODE parameters const float scale = 1.0f; const float offset = 0.0f; - const float thickness = 0.2f; + const float thickness = 2.0f; // Was 0.2f, Larger appears to prevent Av fall-through const int wrap = 0; - int regionsize = (int) Constants.RegionSize + 2; - //Double resolution - //if (((int)Constants.RegionSize) == 256) - // heightMap = ResizeTerrain512Interpolation(heightMap); - - - // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256) - // regionsize = 512; - - float hfmin = 2000; - float hfmax = -2000; + float hfmin = 2000f; + float hfmax = -2000f; + float minele = 0.0f; // Dont allow -ve heights - for (int x = 0; x < heightmapWidthSamples; x++) + uint x = 0; + uint y = 0; + uint xx = 0; + uint yy = 0; + + // load the height samples array from the heightMap + for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257 + { + for ( y = 0; y < heightmapHeightSamples; y++) // 0 to 257 { - for (int y = 0; y < heightmapHeightSamples; y++) - { - int xx = Util.Clip(x - 1, 0, regionsize - 1); - int yy = Util.Clip(y - 1, 0, regionsize - 1); - - - float val= heightMap[yy * (int)Constants.RegionSize + xx]; - _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val; - - hfmin = (val < hfmin) ? val : hfmin; - hfmax = (val > hfmax) ? val : hfmax; - } + xx = x - 1; + if (xx < 0) xx = 0; + if (xx > (regionsize - 1)) xx = regionsize - 1; + + yy = y - 1; + if (yy < 0) yy = 0; + if (yy > (regionsize - 1)) yy = regionsize - 1; + // Input xx = 0 0 1 2 ..... 254 255 255 256 total in + // Output x = 0 1 2 3 ..... 255 256 257 258 total out + float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255> + if (val < minele) val = minele; + _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> + hfmin = (val < hfmin) ? val : hfmin; + hfmax = (val > hfmax) ? val : hfmax; } + } - - - lock (OdeLock) { IntPtr GroundGeom = IntPtr.Zero; @@ -3504,19 +3486,17 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceRemove(space, GroundGeom); d.GeomDestroy(GroundGeom); } - } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1, - (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale, - offset, thickness, wrap); + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, + heightmapWidth, heightmapHeight, (int)heightmapWidthSamples, + (int)heightmapHeightSamples, scale, offset, thickness, wrap); d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); GroundGeom = d.CreateHeightfield(space, HeightmapData, 1); if (GroundGeom != IntPtr.Zero) { d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); - } geom_name_map[GroundGeom] = "Terrain"; @@ -3534,7 +3514,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)) - 1, (pOffset.Y + ((int)Constants.RegionSize * 0.5f)) - 1, 0); + d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0); IntPtr testGround = IntPtr.Zero; if (RegionTerrain.TryGetValue(pOffset, out testGround)) { @@ -3542,7 +3522,6 @@ namespace OpenSim.Region.Physics.OdePlugin } RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); TerrainHeightFieldHeights.Add(GroundGeom,_heightmap); - } }