From ec6347f987cc1e42761ff9bd4832da4f999401f0 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 20 Apr 2012 03:17:36 +0100 Subject: ubitODE - again avatar/terrain collision. Reduce new viewers interpolators efects reporting null velocity and aceleration when stopped near the right position, where they can still have instantanius large values that can get magnified by interpolators, specially using diferent timing estimation. --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics/UbitOdePlugin') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index ec4be58..f4aa231 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -862,12 +862,12 @@ namespace OpenSim.Region.Physics.OdePlugin float depth = terrainheight - chrminZ; if (!flying) { - vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 60; + vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50; } else - vec.Z = depth * PID_P * 60; + vec.Z = depth * PID_P * 50; - if (depth < 0.2f) + if (depth < 0.1f) { m_iscolliding = true; m_colliderfilter = 2; @@ -1009,9 +1009,17 @@ namespace OpenSim.Region.Physics.OdePlugin // update our local ideia of position velocity and aceleration _position = localpos; - _acceleration = _velocity; // previus velocity - _velocity = vel; - _acceleration = (vel - _acceleration) / timeStep; + if (_zeroFlag) + { + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + } + else + { + _acceleration = _velocity; // previus velocity + _velocity = vel; + _acceleration = (vel - _acceleration) / timeStep; + } } -- cgit v1.1 From 2c7f03592571292ea3a563addb1c8fa6af1f6c59 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 20 Apr 2012 04:49:23 +0100 Subject: ubitODE: - Change triangles used in terrain height estimation --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 39 +++++++++++++++++------- 1 file changed, 28 insertions(+), 11 deletions(-) (limited to 'OpenSim/Region/Physics/UbitOdePlugin') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 9ca2d3f..72ac605 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2007,7 +2007,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else // out world use external height { - ix = regsize - 1; + ix = regsize - 2; dx = 0; } if (y < regsize - 1) @@ -2017,7 +2017,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - iy = regsize - 1; + iy = regsize - 2; dy = 0; } } @@ -2034,7 +2034,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else // out world use external height { - iy = regsize - 1; + iy = regsize - 2; dy = 0; } if (y < regsize - 1) @@ -2044,7 +2044,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - ix = regsize - 1; + ix = regsize - 2; dx = 0; } } @@ -2057,18 +2057,35 @@ namespace OpenSim.Region.Physics.OdePlugin iy += ix; // all indexes have iy + ix float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + /* + if ((dx + dy) <= 1.0f) + { + h0 = ((float)heights[iy]); // 0,0 vertice + h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + } + else + { + h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice + h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 + h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + } + */ + h0 = ((float)heights[iy]); // 0,0 vertice - if ((dx + dy) <= 1.0f) + if ((dy > dx)) { - h0 = ((float)heights[iy]); // 0,0 vertice - h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 - h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + iy += regsize; + h2 = (float)heights[iy]; // 0,1 vertice + h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 + h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 } else { - h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice - h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 - h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + iy++; + h2 = (float)heights[iy]; // vertice 1,0 + h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 } return h0 + h1 + h2; -- cgit v1.1 From 190e7a43349d1adf28d52b0e7b74fc91a76fbbdb Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 21 Apr 2012 05:16:54 +0100 Subject: ubitODE: - don't try to hover underground unless volumedetector (that doesn't colide with it) --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 140 ++++++++++++++++-------- 1 file changed, 93 insertions(+), 47 deletions(-) (limited to 'OpenSim/Region/Physics/UbitOdePlugin') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 5467b9f..49766f8 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -68,9 +68,17 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_fakeisphysical; private bool m_isphantom; private bool m_fakeisphantom; + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively protected bool m_building; protected bool m_forcePosOrRotation; + private bool m_iscolliding; + + internal bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + internal bool m_outbounds; private Quaternion m_lastorientation = new Quaternion(); private Quaternion _orientation; @@ -153,14 +161,6 @@ namespace OpenSim.Region.Physics.OdePlugin private List childrenPrim = new List(); - private bool m_iscolliding; - - public bool m_isSelected; - private bool m_delaySelect; - private bool m_lastdoneSelected; - public bool m_outbounds; - - internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively private bool m_throttleUpdates; private int throttleCounter; @@ -223,9 +223,12 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool IsVolumeDtc { - set { return; } - get { return m_isVolumeDetect; } - + get { return m_fakeisVolumeDetect; } + set + { + m_fakeisVolumeDetect = value; + AddChange(changes.VolumeDtc, value); + } } @@ -234,10 +237,7 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_fakeisphantom; } set { - m_fakeisphantom = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - + m_fakeisphantom = value; AddChange(changes.Phantom, value); } } @@ -427,7 +427,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SetVolumeDetect(int param) { - AddChange(changes.VolumeDtc, (param != 0)); + m_fakeisVolumeDetect = (param != 0); + AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); } public override Vector3 GeometricCenter @@ -958,6 +959,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_fakeisphysical = m_isphysical; m_isVolumeDetect = false; + m_fakeisVolumeDetect = false; m_force = Vector3.Zero; @@ -1066,7 +1068,7 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_collisionCategories = CollisionCategories.Selected; prm.m_collisionFlags = 0; } - else if (prm.IsVolumeDtc) + else if (prm.m_isVolumeDetect) { prm.m_collisionCategories = CollisionCategories.VolumeDtc; if (m_isphysical) @@ -1445,14 +1447,14 @@ namespace OpenSim.Region.Physics.OdePlugin hasOOBoffsetFromMesh = false; CalcPrimBodyData(); } - +/* private void ChildSetGeom(OdePrim odePrim) { // well.. DestroyBody(); MakeBody(); } - +*/ //sets non physical prim m_targetSpace to right space in spaces grid for static prims // should only be called for non physical prims unless they are becoming non physical private void SetInStaticSpace(OdePrim prim) @@ -2650,6 +2652,31 @@ namespace OpenSim.Region.Physics.OdePlugin ApplyCollisionCatFlags(); } +/* not in use + internal void ChildSelectedChange(bool childSelect) + { + if(childPrim) + return; + + if (childSelect == m_isSelected) + return; + + if (childSelect) + { + DoSelectedStatus(true); + } + + else + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.m_isSelected) + return; + } + DoSelectedStatus(false); + } + } +*/ private void changeSelectedStatus(bool newval) { if (m_lastdoneSelected == newval) @@ -2669,6 +2696,12 @@ namespace OpenSim.Region.Physics.OdePlugin private void DoSelectedStatus(bool newval) { + if (m_isSelected == newval) + { + resetCollisionAccounting(); + return; + } + m_isSelected = newval; Stop(); @@ -2706,6 +2739,9 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_delaySelect = false; } } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(true); + if (prim_geom != null) { @@ -2741,8 +2777,13 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - if (!childPrim && Body != IntPtr.Zero && !m_disabled) - d.BodyEnable(Body); + if (!childPrim) + { + if (Body != IntPtr.Zero && !m_disabled) + d.BodyEnable(Body); + } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(false); UpdateCollisionCatFlags(); ApplyCollisionCatFlags(); @@ -3145,6 +3186,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeVolumedetetion(bool newVolDtc) { m_isVolumeDetect = newVolDtc; + m_fakeisVolumeDetect = newVolDtc; UpdateCollisionCatFlags(); ApplyCollisionCatFlags(); } @@ -3370,7 +3412,7 @@ namespace OpenSim.Region.Physics.OdePlugin //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); + // d.BodyAddForce(Body, 0, 0, fz); return; } else @@ -3419,14 +3461,17 @@ namespace OpenSim.Region.Physics.OdePlugin { case PIDHoverType.Ground: m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; case PIDHoverType.GroundAndWater: m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); m_waterHeight = _parent_scene.GetWaterLevel(); if (m_groundHeight > m_waterHeight) { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + if (m_PIDHoverHeight > 0 || m_isVolumeDetect) + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + else + m_targetHoverHeight = m_groundHeight; } else { @@ -3436,34 +3481,35 @@ namespace OpenSim.Region.Physics.OdePlugin } // end switch (m_PIDHoverType) + // don't go underground unless volumedetector + + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + { + fz = (m_targetHoverHeight - pos.Z) * (PID_G - m_PIDHoverTau) * timestep; - _target_velocity = - new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control + // if velocity is zero, use position control; otherwise, velocity control - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' + if (Math.Abs(fz) < 0.1f) + { + // keep track of where we stopped. No more slippin' & slidin' - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - // ? d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; - // We're flying and colliding with something - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + // We're flying and colliding with something + fz = ((fz - vel.Z) * (PID_D)); + } } } else -- cgit v1.1 From e0f81e24000df3a969cd313d008194d8226272ff Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 23 Apr 2012 01:47:11 +0100 Subject: ubitODE - several changes... --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 + OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 676 ++++++++++----------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 131 ++-- 3 files changed, 379 insertions(+), 431 deletions(-) (limited to 'OpenSim/Region/Physics/UbitOdePlugin') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index f4aa231..6ffcb9e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -686,6 +686,9 @@ namespace OpenSim.Region.Physics.OdePlugin Body = d.BodyCreate(_parent_scene.world); + _zeroFlag = false; + m_pidControllerActive = true; + d.BodySetAutoDisableFlag(Body, false); d.BodySetPosition(Body, npositionX, npositionY, npositionZ); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 49766f8..7c0bbef 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -98,10 +98,12 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_forceacc; private Vector3 m_angularForceacc; + private float m_invTimeStep = 50.0f; + private float m_timeStep = .02f; + + private Vector3 m_PIDTarget; private float m_PIDTau; - private float PID_D = 35f; - private float PID_G = 25f; private bool m_usePID; // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), @@ -632,7 +634,6 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 pv = Vector3.Zero; if (_zeroFlag) return pv; - m_lastUpdateSent = false; if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) return pv; @@ -686,12 +687,50 @@ namespace OpenSim.Region.Physics.OdePlugin } public override bool PIDActive { set { m_usePID = value; } } - public override float PIDTau { set { m_PIDTau = value; } } + public override float PIDTau + { + set + { + if (value <= 0) + m_PIDTau = 0; + else + { + float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE); + if (value < mint) + m_PIDTau = mint; + else + m_PIDTau = value; + } + } + } - public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override float PIDHoverHeight + { + set + { + m_PIDHoverHeight = value; + if (value == 0) + m_useHoverPID = false; + } + } public override bool PIDHoverActive { set { m_useHoverPID = value; } } public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } - public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + public override float PIDHoverTau + { + set + { + if (value <= 0) + m_PIDHoverTau = 0; + else + { + float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE); + if (value < mint) + m_PIDHoverTau = mint; + else + m_PIDHoverTau = value; + } + } + } public override Quaternion APIDTarget { set { return; } } @@ -912,8 +951,9 @@ namespace OpenSim.Region.Physics.OdePlugin _position = pos; givefakepos = 0; - PID_D = parent_scene.bodyPIDD; - PID_G = parent_scene.bodyPIDG; + m_timeStep = parent_scene.ODE_STEPSIZE; + m_invTimeStep = 1f / m_timeStep; + m_density = parent_scene.geomDefaultDensity; // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; body_autodisable_frames = parent_scene.bodyFramesAutoDisable; @@ -2696,12 +2736,6 @@ namespace OpenSim.Region.Physics.OdePlugin private void DoSelectedStatus(bool newval) { - if (m_isSelected == newval) - { - resetCollisionAccounting(); - return; - } - m_isSelected = newval; Stop(); @@ -2970,7 +3004,6 @@ namespace OpenSim.Region.Physics.OdePlugin givefakeori--; if (givefakeori < 0) givefakeori = 0; - resetCollisionAccounting(); } @@ -3125,9 +3158,10 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(Body); } - private void changeAddForce(Vector3 force) + + private void changeAddImpulse(Vector3 impulse) { - m_forceacc += force; + m_forceacc += impulse * m_invTimeStep; if (!m_isSelected) { lock (this) @@ -3146,9 +3180,10 @@ namespace OpenSim.Region.Physics.OdePlugin } } - private void changeAddAngularForce(Vector3 aforce) + // actually angular impulse + private void changeAddAngularImpulse(Vector3 aimpulse) { - m_angularForceacc += aforce; + m_angularForceacc += aimpulse * m_invTimeStep; if (!m_isSelected) { lock (this) @@ -3271,330 +3306,246 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim && m_isphysical && Body != IntPtr.Zero && !m_disabled && !m_isSelected && !m_building && !m_outbounds) { - if (d.BodyIsEnabled(Body)) + if (!d.BodyIsEnabled(Body)) { - float timestep = _parent_scene.ODE_STEPSIZE; - - // check outside region - d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - - if (lpos.Z < -100 || lpos.Z > 100000f) - { - m_outbounds = true; - - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); + // let vehicles sleep + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) return; - } - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } - - if (m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; - - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - base.RequestPhysicsterseUpdate(); + if (++bodydisablecontrol < 20) return; - } - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(); - } - else - { - float fx = 0; - float fy = 0; - float fz = 0; - - float m_mass = _mass; - - // fz = 0f; - //m_log.Info(m_collisionFlags.ToString()); - if (m_usePID) - { + bodydisablecontrol = 0; + d.BodyEnable(Body); + } - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + // check outside region + d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; - // if velocity is zero, use position control; otherwise, velocity control + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - // d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + base.RequestPhysicsterseUpdate(); - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + throttleCounter = 0; + _zeroFlag = true; - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } // end if (m_usePID) + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID) - { - //Console.WriteLine("Hover " + Name); + if (m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; - // If we're using the PID controller, then we have no gravity + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; - // no lock; for now it's only called from within Simulate() + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + return; + } - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } + float fx = 0; + float fy = 0; + float fz = 0; - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); + float m_mass = _mass; - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + if (m_usePID && m_PIDTau > 0) + { + // for now position error + _target_velocity = + new Vector3( + (m_PIDTarget.X - lpos.X), + (m_PIDTarget.Y - lpos.Y), + (m_PIDTarget.Z - lpos.Z) + ); - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - if (m_PIDHoverHeight > 0 || m_isVolumeDetect) - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - else - m_targetHoverHeight = m_groundHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) + { + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + return; + } + else + { + _zeroFlag = false; - } // end switch (m_PIDHoverType) + float tmp = 1 / m_PIDTau; + _target_velocity *= tmp; - // don't go underground unless volumedetector - - if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) - { - fz = (m_targetHoverHeight - pos.Z) * (PID_G - m_PIDHoverTau) * timestep; + // apply limits + tmp = _target_velocity.Length(); + if (tmp > 50.0f) + { + tmp = 50 / tmp; + _target_velocity *= tmp; + } + else if (tmp < 0.05f) + { + tmp = 0.05f / tmp; + _target_velocity *= tmp; + } - // if velocity is zero, use position control; otherwise, velocity control + d.Vector3 vel = d.BodyGetLinearVel(Body); + fx = (_target_velocity.X - vel.X) * m_invTimeStep; + fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; + fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; + } + } // end if (m_usePID) - if (Math.Abs(fz) < 0.1f) - { - // keep track of where we stopped. No more slippin' & slidin' + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) + { - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - // ? d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); - // We're flying and colliding with something - fz = ((fz - vel.Z) * (PID_D)); - } - } - } - else - { - float b = (1.0f - m_buoyancy); - fx = _parent_scene.gravityx * b; - fy = _parent_scene.gravityy * b; - fz = _parent_scene.gravityz * b; - } + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; + case PIDHoverType.GroundAndWater: + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + else + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + break; + } // end switch (m_PIDHoverType) - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; + // don't go underground unless volumedetector - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + { + d.Vector3 vel = d.BodyGetLinearVel(Body); - m_forceacc = Vector3.Zero; + fz = (m_targetHoverHeight - lpos.Z); - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) + // if error is zero, use position control; otherwise, velocity control + if (Math.Abs(fz) < 0.01f) { - d.BodyAddForce(Body, fx, fy, fz); - //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + return; } + else + { + _zeroFlag = false; + fz /= m_PIDHoverTau; - Vector3 trq; + float tmp = Math.Abs(fz); + if (tmp > 50) + fz = 50 * Math.Sign(fz); + else if (tmp < 0.1) + fz = 0.1f * Math.Sign(fz); - trq = _torque; - trq += m_angularForceacc; - m_angularForceacc = Vector3.Zero; - if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) - { - d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + fz = ((fz - vel.Z) * m_invTimeStep); } } + } + else + { + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } - // update our ideia of velocities and acelerations - d.Quaternion ori; - d.Vector3 dtmpu; - - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - - d.GeomCopyQuaternion(prim_geom, out ori); - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; - _acceleration = _velocity; + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; - dtmpu = d.BodyGetLinearVel(Body); - _velocity.X = dtmpu.X; - _velocity.Y = dtmpu.Y; - _velocity.Z = dtmpu.Z; + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; - float invts = 1 / timestep; - _acceleration = (_velocity - _acceleration) * invts; + m_forceacc = Vector3.Zero; - dtmpu = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmpu.X; - m_rotationalVelocity.Y = dtmpu.Y; - m_rotationalVelocity.Z = dtmpu.Z; + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); } - else // body disabled/sleeping - { - // let vehicles sleep - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - return; + Vector3 trq; - if (++bodydisablecontrol < 20) - return; - - bodydisablecontrol = 0; - d.BodyEnable(Body); - return; + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); } } else @@ -3606,92 +3557,113 @@ namespace OpenSim.Region.Physics.OdePlugin } } - - public void UpdatePositionAndVelocity(float simulatedtime) + public void UpdatePositionAndVelocity() { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null && !m_disabled && !m_building && !m_outbounds) + if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) { - if (Body != IntPtr.Zero) + if (d.BodyIsEnabled(Body) || !_zeroFlag) { bool lastZeroFlag = _zeroFlag; - if ((Math.Abs(m_lastposition.X - _position.X) < 0.01) - && (Math.Abs(m_lastposition.Y - _position.Y) < 0.01) - && (Math.Abs(m_lastposition.Z - _position.Z) < 0.01) - && (Math.Abs(m_lastorientation.X - _orientation.X) < 0.0001) - && (Math.Abs(m_lastorientation.Y - _orientation.Y) < 0.0001) - && (Math.Abs(m_lastorientation.Z - _orientation.Z) < 0.0001) + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + + // decide if moving + // use positions since this are integrated quantities + // tolerance values depende a lot on simulation noise... + // use simple math.abs since we dont need to be exact + + if ( + (Math.Abs(_position.X - lpos.X) < 0.001f) + && (Math.Abs(_position.Y - lpos.Y) < 0.001f) + && (Math.Abs(_position.Z - lpos.Z) < 0.001f) + && (Math.Abs(_orientation.X - ori.X) < 0.0001f) + && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f) + && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W ) { _zeroFlag = true; - m_throttleUpdates = false; } else - { _zeroFlag = false; - m_lastUpdateSent = false; - } - if (_zeroFlag) + // update velocities and aceleration + if (!(_zeroFlag && lastZeroFlag)) { - m_lastposition = _position; - m_lastorientation = _orientation; + d.Vector3 vel = d.BodyGetLinearVel(Body); - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; + _acceleration = _velocity; - if (!m_lastUpdateSent) + if ((Math.Abs(vel.X) < 0.001f) && + (Math.Abs(vel.Y) < 0.001f) && + (Math.Abs(vel.Z) < 0.001f)) { - m_throttleUpdates = false; - throttleCounter = 0; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; + _velocity = Vector3.Zero; + float t = -m_invTimeStep; + _acceleration = _acceleration * t; } - } - else - { - if (lastZeroFlag != _zeroFlag) + else { - base.RequestPhysicsterseUpdate(); + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + _acceleration = (_velocity - _acceleration) * m_invTimeStep; } - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + if ((Math.Abs(_acceleration.X) < 0.01f) && + (Math.Abs(_acceleration.Y) < 0.01f) && + (Math.Abs(_acceleration.Z) < 0.01f)) { - m_lastposition = _position; - m_lastorientation = _orientation; - m_lastVelocity = _velocity; - base.RequestPhysicsterseUpdate(); + _acceleration = Vector3.Zero; + } + + if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && + (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && + (Math.Abs(_orientation.Z - ori.Z) < 0.0001) + ) + { + m_rotationalVelocity = Vector3.Zero; } else { - throttleCounter++; + vel = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = vel.X; + m_rotationalVelocity.Y = vel.Y; + m_rotationalVelocity.Z = vel.Z; } } - } - else if (!m_lastUpdateSent || !_zeroFlag) - { - // Not a body.. so Make sure the client isn't interpolating - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - m_lastVelocity = Vector3.Zero; - - _zeroFlag = true; - if (!m_lastUpdateSent) + if (_zeroFlag) { - m_throttleUpdates = false; - throttleCounter = 0; - - base.RequestPhysicsterseUpdate(); + if (lastZeroFlag) + { + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + } - m_lastUpdateSent = true; + if (!m_lastUpdateSent) + { + base.RequestPhysicsterseUpdate(); + if (lastZeroFlag) + m_lastUpdateSent = true; + } + return; } + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + base.RequestPhysicsterseUpdate(); + m_lastUpdateSent = false; } } } @@ -3849,11 +3821,11 @@ namespace OpenSim.Region.Physics.OdePlugin break; case changes.AddForce: - changeAddForce((Vector3)arg); + changeAddImpulse((Vector3)arg); break; case changes.AddAngForce: - changeAddAngularForce((Vector3)arg); + changeAddAngularImpulse((Vector3)arg); break; case changes.AngLock: diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 72ac605..1d9fa93 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -521,7 +521,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldSetAngularDamping(world, 0.001f); d.WorldSetAngularDampingThreshold(world, 0f); d.WorldSetLinearDampingThreshold(world, 0f); - d.WorldSetMaxAngularSpeed(world, 256f); + d.WorldSetMaxAngularSpeed(world, 100f); d.WorldSetCFM(world,1e-6f); // a bit harder than default //d.WorldSetCFM(world, 1e-4f); // a bit harder than default @@ -1685,17 +1685,6 @@ namespace OpenSim.Region.Physics.OdePlugin /// public override float Simulate(float timeStep) { - int statstart; - int statchanges = 0; - int statchmove = 0; - int statactmove = 0; - int statray = 0; - int statcol = 0; - int statstep = 0; - int statmovchar = 0; - int statmovprim; - int totjcontact = 0; - // acumulate time so we can reduce error step_time += timeStep; @@ -1738,8 +1727,6 @@ namespace OpenSim.Region.Physics.OdePlugin { try { - statstart = Util.EnvironmentTickCount(); - // clear pointer/counter to contacts to pass into joints m_global_contactcount = 0; @@ -1778,17 +1765,39 @@ namespace OpenSim.Region.Physics.OdePlugin } - statchanges += Util.EnvironmentTickCountSubtract(statstart); + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(ODE_STEPSIZE, defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + } + } + + // Move other active objects + lock (_activegroups) + { + foreach (OdePrim aprim in _activegroups) + { + aprim.Move(); + } + } - statactmove += Util.EnvironmentTickCountSubtract(statstart); //if ((framecount % m_randomizeWater) == 0) // randomizeWater(waterlevel); m_rayCastManager.ProcessQueuedRequests(); - statray += Util.EnvironmentTickCountSubtract(statstart); collision_optimized(); - statcol += Util.EnvironmentTickCountSubtract(statstart); lock (_collisionEventPrim) { @@ -1813,38 +1822,39 @@ namespace OpenSim.Region.Physics.OdePlugin } } + // do a ode simulation step d.WorldQuickStep(world, ODE_STEPSIZE); - statstep += Util.EnvironmentTickCountSubtract(statstart); + d.JointGroupEmpty(contactgroup); + + // update managed ideia of physical data and do updates to core + /* + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); + + actor.UpdatePositionAndVelocity(); + } + } + } + */ - // Move characters - lock (_characters) + lock (_activegroups) { - List defects = new List(); - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - actor.Move(ODE_STEPSIZE, defects); - } - if (defects.Count != 0) { - foreach (OdeCharacter defect in defects) + foreach (OdePrim actor in _activegroups) { - RemoveCharacter(defect); + if (actor.IsPhysical) + { + actor.UpdatePositionAndVelocity(); + } } } } - statchmove += Util.EnvironmentTickCountSubtract(statstart); - - // Move other active objects - lock (_activegroups) - { - foreach (OdePrim aprim in _activegroups) - { - aprim.Move(); - } - } - - //ode.dunlock(world); } catch (Exception e) { @@ -1852,32 +1862,11 @@ namespace OpenSim.Region.Physics.OdePlugin // ode.dunlock(world); } - d.JointGroupEmpty(contactgroup); - totjcontact += m_global_contactcount; step_time -= ODE_STEPSIZE; nodeframes++; } - statstart = Util.EnvironmentTickCount(); - -/* -// now included in characters move() and done at ode rate -// maybe be needed later if we need to do any extra work at hearbeat rate - lock (_characters) - { - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - { - if (actor.bad) - m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); - - actor.UpdatePositionAndVelocity(); - } - } - } -*/ lock (_badCharacter) { if (_badCharacter.Count > 0) @@ -1890,22 +1879,6 @@ namespace OpenSim.Region.Physics.OdePlugin _badCharacter.Clear(); } } - statmovchar = Util.EnvironmentTickCountSubtract(statstart); - - lock (_activegroups) - { - { - foreach (OdePrim actor in _activegroups) - { - if (actor.IsPhysical) - { - actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE); - } - } - } - } - - statmovprim = Util.EnvironmentTickCountSubtract(statstart); int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); -- cgit v1.1 From 5a8fdc8a0b79c14382872571b113b5c5559083c4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 23 Apr 2012 20:16:53 +0100 Subject: ubitODE - do own timing control (as chODE does) until heartbeat does it right --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 8 ++-- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 47 +++++++++++++++--------- 2 files changed, 34 insertions(+), 21 deletions(-) (limited to 'OpenSim/Region/Physics/UbitOdePlugin') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 7c0bbef..dc6c18d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -695,7 +695,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_PIDTau = 0; else { - float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE); + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); if (value < mint) m_PIDTau = mint; else @@ -723,7 +723,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_PIDHoverTau = 0; else { - float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE); + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); if (value < mint) m_PIDHoverTau = mint; else @@ -801,7 +801,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (force.IsFinite()) { - AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); + AddChange(changes.AddForce, force * m_invTimeStep); } else { @@ -814,7 +814,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (force.IsFinite()) { - AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); + AddChange(changes.AddAngForce, force * m_invTimeStep); } else { diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 1d9fa93..cf74f14 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -189,9 +189,12 @@ namespace OpenSim.Region.Physics.OdePlugin private const uint m_regionHeight = Constants.RegionSize; public float ODE_STEPSIZE = 0.020f; + public float HalfOdeStep = 0.01f; private float metersInSpace = 25.6f; private float m_timeDilation = 1.0f; + DateTime m_lastframe; + public float gravityx = 0f; public float gravityy = 0f; public float gravityz = -9.8f; @@ -485,6 +488,8 @@ namespace OpenSim.Region.Physics.OdePlugin } } + HalfOdeStep = ODE_STEPSIZE * 0.5f; + ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); @@ -564,6 +569,7 @@ namespace OpenSim.Region.Physics.OdePlugin // let this now be real maximum values spaceGridMaxX--; spaceGridMaxY--; + m_lastframe = DateTime.UtcNow; } internal void waitForSpaceUnlock(IntPtr space) @@ -1685,24 +1691,30 @@ namespace OpenSim.Region.Physics.OdePlugin /// public override float Simulate(float timeStep) { + + DateTime now = DateTime.UtcNow; + TimeSpan SinceLastFrame = now - m_lastframe; + m_lastframe = now; + timeStep = (float)SinceLastFrame.TotalSeconds; + // acumulate time so we can reduce error step_time += timeStep; - if (step_time < ODE_STEPSIZE) + if (step_time < HalfOdeStep) return 0; - if (framecount >= int.MaxValue) + if (framecount < 0) framecount = 0; framecount++; - int curphysiteractions = m_physicsiterations; + int curphysiteractions; + // if in trouble reduce step resolution if (step_time >= m_SkipFramesAtms) - { - // if in trouble reduce step resolution - curphysiteractions /= 2; - } + curphysiteractions = m_physicsiterations / 2; + else + curphysiteractions = m_physicsiterations; int nodeframes = 0; @@ -1722,8 +1734,7 @@ namespace OpenSim.Region.Physics.OdePlugin base.TriggerPhysicsBasedRestart(); } - - while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever + while (step_time >= HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever { try { @@ -1905,15 +1916,17 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); } - // think time dilation is not a physics issue alone.. but ok let's fake something - if (step_time < ODE_STEPSIZE) // we did the required loops + // think time dilation as to do with dinamic step size that we dont' have + // even so tell something to world + if (nodeframes < 10) // we did the requested loops m_timeDilation = 1.0f; - else - { // we didn't forget the lost ones and let user know something - m_timeDilation = 1 - step_time / timeStep; - if (m_timeDilation < 0) - m_timeDilation = 0; - step_time = 0; + else if (step_time > 0) + { + m_timeDilation = timeStep / step_time; + if (m_timeDilation > 1) + m_timeDilation = 1; + if (step_time > m_SkipFramesAtms) + step_time = 0; } } -- cgit v1.1