From ec6347f987cc1e42761ff9bd4832da4f999401f0 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 20 Apr 2012 03:17:36 +0100 Subject: [PATCH 1/7] 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. --- .../Physics/UbitOdePlugin/ODECharacter.cs | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index ec4be58361..f4aa231dc1 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; + } } From 2c7f03592571292ea3a563addb1c8fa6af1f6c59 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 20 Apr 2012 04:49:23 +0100 Subject: [PATCH 2/7] ubitODE: - Change triangles used in terrain height estimation --- .../Region/Physics/UbitOdePlugin/OdeScene.cs | 39 +++++++++++++------ 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 9ca2d3f29b..72ac605d15 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; From 3b56c4445390461ea6cdef7bc9ad8f8b26fc97c1 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 20 Apr 2012 18:51:32 +0100 Subject: [PATCH 3/7] changed seletion code. SOP now knows about parts selection. UI actions are sent to SOP and this reports to SOG. Group is selected if any part is selected.sop.isSelect get() is only used in SOG. Will need to be improved for better performance on largelinksets. *UNTESTED* NEEDS CHECKING for side efects --- .../Framework/Scenes/Scene.PacketHandlers.cs | 66 ++++++++++++++----- .../Framework/Scenes/SceneObjectGroup.cs | 48 +++++++++++--- .../Framework/Scenes/SceneObjectPart.cs | 14 ++++ 3 files changed, 104 insertions(+), 24 deletions(-) diff --git a/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs b/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs index 35ac908b57..3ef1e29782 100644 --- a/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs +++ b/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs @@ -129,27 +129,47 @@ namespace OpenSim.Region.Framework.Scenes /// public void SelectPrim(uint primLocalID, IClientAPI remoteClient) { + /* + SceneObjectPart part = GetSceneObjectPart(primLocalID); + + if (null == part) + return; + + if (part.IsRoot) + { + SceneObjectGroup sog = part.ParentGroup; + sog.SendPropertiesToClient(remoteClient); + + // A prim is only tainted if it's allowed to be edited by the person clicking it. + if (Permissions.CanEditObject(sog.UUID, remoteClient.AgentId) + || Permissions.CanMoveObject(sog.UUID, remoteClient.AgentId)) + { + sog.IsSelected = true; + EventManager.TriggerParcelPrimCountTainted(); + } + } + else + { + part.SendPropertiesToClient(remoteClient); + } + */ SceneObjectPart part = GetSceneObjectPart(primLocalID); if (null == part) return; - if (part.IsRoot) - { - SceneObjectGroup sog = part.ParentGroup; - sog.SendPropertiesToClient(remoteClient); + SceneObjectGroup sog = part.ParentGroup; + if (sog == null) + return; - // A prim is only tainted if it's allowed to be edited by the person clicking it. - if (Permissions.CanEditObject(sog.UUID, remoteClient.AgentId) - || Permissions.CanMoveObject(sog.UUID, remoteClient.AgentId)) - { - sog.IsSelected = true; - EventManager.TriggerParcelPrimCountTainted(); - } - } - else + part.SendPropertiesToClient(remoteClient); + + // A prim is only tainted if it's allowed to be edited by the person clicking it. + if (Permissions.CanEditObject(sog.UUID, remoteClient.AgentId) + || Permissions.CanMoveObject(sog.UUID, remoteClient.AgentId)) { - part.SendPropertiesToClient(remoteClient); + part.IsSelected = true; + EventManager.TriggerParcelPrimCountTainted(); } } @@ -202,7 +222,7 @@ namespace OpenSim.Region.Framework.Scenes SceneObjectPart part = GetSceneObjectPart(primLocalID); if (part == null) return; - + /* // A deselect packet contains all the local prims being deselected. However, since selection is still // group based we only want the root prim to trigger a full update - otherwise on objects with many prims // we end up sending many duplicate ObjectUpdates @@ -237,6 +257,22 @@ namespace OpenSim.Region.Framework.Scenes part.UUID, remoteClient.AgentId)) EventManager.TriggerParcelPrimCountTainted(); } + */ + + bool oldgprSelect = part.ParentGroup.IsSelected; + + // This is wrong, wrong, wrong. Selection should not be + // handled by group, but by prim. Legacy cruft. + // TODO: Make selection flagging per prim! + // + if (Permissions.CanEditObject(part.ParentGroup.UUID, remoteClient.AgentId) + || Permissions.CanMoveObject(part.ParentGroup.UUID, remoteClient.AgentId)) + { + part.IsSelected = false; + if (!part.ParentGroup.IsAttachment && oldgprSelect != part.ParentGroup.IsSelected) + EventManager.TriggerParcelPrimCountTainted(); + } + } public virtual void ProcessMoneyTransferRequest(UUID source, UUID destination, int amount, diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 47020afd2a..0100ab3b5a 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -726,6 +726,11 @@ namespace OpenSim.Region.Framework.Scenes m_isSelected = value; // Tell physics engine that group is selected + // this is not right + // but ode engines should only really need to know about root part + // so they can put entire object simulation on hold and not colliding + // keep as was for now + PhysicsActor pa = m_rootPart.PhysActor; if (pa != null) { @@ -747,6 +752,40 @@ namespace OpenSim.Region.Framework.Scenes } } + public void PartSelectChanged(bool partSelect) + { + // any part selected makes group selected + if (m_isSelected == partSelect) + return; + + if (partSelect) + { + IsSelected = partSelect; +// if (!IsAttachment) +// ScheduleGroupForFullUpdate(); + } + else + { + // bad bad bad 2 heavy for large linksets + // since viewer does send lot of (un)selects + // this needs to be replaced by a specific list or count ? + // but that will require extra code in several places + + SceneObjectPart[] parts = m_parts.GetArray(); + for (int i = 0; i < parts.Length; i++) + { + SceneObjectPart part = parts[i]; + if (part.IsSelected) + return; + } + IsSelected = partSelect; + if (!IsAttachment) + { + ScheduleGroupForFullUpdate(); + } + } + } + private SceneObjectPart m_PlaySoundMasterPrim = null; public SceneObjectPart PlaySoundMasterPrim { @@ -3139,14 +3178,6 @@ namespace OpenSim.Region.Framework.Scenes } } -/* - RootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect); - for (int i = 0; i < parts.Length; i++) - { - if (parts[i] != RootPart) - parts[i].UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect); - } -*/ if (parts.Length > 1) { m_rootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect, true); @@ -3163,7 +3194,6 @@ namespace OpenSim.Region.Framework.Scenes } else m_rootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect, false); - } } diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index c73fc986bd..c9659cba88 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -304,6 +304,9 @@ namespace OpenSim.Region.Framework.Scenes protected float m_friction = 0.6f; // wood protected float m_bounce = 0.5f; // wood + + protected bool m_isSelected = false; + /// /// Stores media texture data /// @@ -577,6 +580,16 @@ namespace OpenSim.Region.Framework.Scenes } } + public bool IsSelected + { + get { return m_isSelected; } + set + { + m_isSelected = value; + if (ParentGroup != null) + ParentGroup.PartSelectChanged(value); + } + } public Dictionary CollisionFilter @@ -1907,6 +1920,7 @@ namespace OpenSim.Region.Framework.Scenes dupe.m_rezzed = m_rezzed; dupe.m_UndoRedo = null; + dupe.m_isSelected = false; dupe.IgnoreUndoUpdate = false; dupe.Undoing = false; From 190e7a43349d1adf28d52b0e7b74fc91a76fbbdb Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 21 Apr 2012 05:16:54 +0100 Subject: [PATCH 4/7] ubitODE: - don't try to hover underground unless volumedetector (that doesn't colide with it) --- .../Region/Physics/UbitOdePlugin/ODEPrim.cs | 142 ++++++++++++------ 1 file changed, 94 insertions(+), 48 deletions(-) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 5467b9f85e..49766f8f9a 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) - - _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 (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + // don't go underground unless volumedetector + + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) { - // keep track of where we stopped. No more slippin' & slidin' + fz = (m_targetHoverHeight - pos.Z) * (PID_G - m_PIDHoverTau) * timestep; - // 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 + // if velocity is zero, use position control; otherwise, velocity control - 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; + if (Math.Abs(fz) < 0.1f) + { + // keep track of where we stopped. No more slippin' & slidin' - // We're flying and colliding with something - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + // 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; + + // We're flying and colliding with something + fz = ((fz - vel.Z) * (PID_D)); + } } } else From 26fd1e3a0c77a5b88429513b47cdd6d16d5867f5 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 22 Apr 2012 03:08:47 +0100 Subject: [PATCH 5/7] fix a bug i added fixing another.... --- OpenSim/Region/Framework/Scenes/ScenePresence.cs | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs index 121179219e..108b0448ef 100644 --- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs +++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs @@ -753,9 +753,10 @@ namespace OpenSim.Region.Framework.Scenes if (m_movementAnimationUpdateCounter >= 2) { m_movementAnimationUpdateCounter = 0; - if (Animator != null && ParentID == 0) // skip it if sitting + if (Animator != null) { - Animator.UpdateMovementAnimations(); + if(ParentID == 0) // skip it if sitting + Animator.UpdateMovementAnimations(); } else { @@ -1400,6 +1401,7 @@ namespace OpenSim.Region.Framework.Scenes { // Vector3 posAdjusted = m_pos + HEAD_ADJUSTMENT; // m_scene.PhysicsScene.RaycastWorld(m_pos, Vector3.Normalize(CameraPosition - posAdjusted), Vector3.Distance(CameraPosition, posAdjusted) + 0.3f, RayCastCameraCallback); + Vector3 posAdjusted = AbsolutePosition + HEAD_ADJUSTMENT; Vector3 distTocam = CameraPosition - posAdjusted; float distTocamlen = distTocam.Length(); @@ -1408,6 +1410,7 @@ namespace OpenSim.Region.Framework.Scenes distTocam *= 1.0f / distTocamlen; m_scene.PhysicsScene.RaycastWorld(posAdjusted, distTocam, distTocamlen + 0.3f, RayCastCameraCallback); } + } } From e0f81e24000df3a969cd313d008194d8226272ff Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 23 Apr 2012 01:47:11 +0100 Subject: [PATCH 6/7] ubitODE - several changes... --- .../Physics/UbitOdePlugin/ODECharacter.cs | 3 + .../Region/Physics/UbitOdePlugin/ODEPrim.cs | 766 +++++++++--------- .../Region/Physics/UbitOdePlugin/OdeScene.cs | 131 ++- 3 files changed, 424 insertions(+), 476 deletions(-) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index f4aa231dc1..6ffcb9e145 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 49766f8f9a..7c0bbef99c 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,319 +3306,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim && m_isphysical && Body != IntPtr.Zero && !m_disabled && !m_isSelected && !m_building && !m_outbounds) { - 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); - 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(); - 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) - { - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - 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) - ); - - // 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' - - // 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; - - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } // end if (m_usePID) - - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID) - { - //Console.WriteLine("Hover " + Name); - - // If we're using the PID controller, then we have no gravity - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - - // 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); - - 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; - - } // 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; - - // if velocity is zero, use position control; otherwise, velocity control - - 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 - - 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 = ((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; - } - - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; - - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; - - m_forceacc = Vector3.Zero; - - //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); - } - - Vector3 trq; - - 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); - } - } - - // 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; - - _acceleration = _velocity; - - dtmpu = d.BodyGetLinearVel(Body); - _velocity.X = dtmpu.X; - _velocity.Y = dtmpu.Y; - _velocity.Z = dtmpu.Z; - - float invts = 1 / timestep; - _acceleration = (_velocity - _acceleration) * invts; - - dtmpu = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmpu.X; - m_rotationalVelocity.Y = dtmpu.Y; - m_rotationalVelocity.Z = dtmpu.Z; - } - - else // body disabled/sleeping + if (!d.BodyIsEnabled(Body)) { // let vehicles sleep if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) @@ -3594,8 +3317,236 @@ namespace OpenSim.Region.Physics.OdePlugin bodydisablecontrol = 0; d.BodyEnable(Body); + } + + // 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(); + + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); 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(); + return; + } + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + return; + } + + float fx = 0; + float fy = 0; + float fz = 0; + + float m_mass = _mass; + + 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) + ); + + 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; + + float tmp = 1 / m_PIDTau; + _target_velocity *= tmp; + + // 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; + } + + 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) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) + { + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); + + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + + 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) + + // don't go underground unless volumedetector + + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + { + d.Vector3 vel = d.BodyGetLinearVel(Body); + + fz = (m_targetHoverHeight - lpos.Z); + + // if error is zero, use position control; otherwise, velocity control + if (Math.Abs(fz) < 0.01f) + { + d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + return; + } + else + { + _zeroFlag = false; + fz /= m_PIDHoverTau; + + float tmp = Math.Abs(fz); + if (tmp > 50) + fz = 50 * Math.Sign(fz); + else if (tmp < 0.1) + fz = 0.1f * Math.Sign(fz); + + 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; + } + + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; + + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; + + m_forceacc = Vector3.Zero; + + //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); + } + + Vector3 trq; + + 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 { // is not physical, or is not a body or is selected @@ -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; + + // update velocities and aceleration + if (!(_zeroFlag && lastZeroFlag)) + { + d.Vector3 vel = d.BodyGetLinearVel(Body); + + _acceleration = _velocity; + + if ((Math.Abs(vel.X) < 0.001f) && + (Math.Abs(vel.Y) < 0.001f) && + (Math.Abs(vel.Z) < 0.001f)) + { + _velocity = Vector3.Zero; + float t = -m_invTimeStep; + _acceleration = _acceleration * t; + } + else + { + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + _acceleration = (_velocity - _acceleration) * m_invTimeStep; + } + + if ((Math.Abs(_acceleration.X) < 0.01f) && + (Math.Abs(_acceleration.Y) < 0.01f) && + (Math.Abs(_acceleration.Z) < 0.01f)) + { + _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 + { + vel = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = vel.X; + m_rotationalVelocity.Y = vel.Y; + m_rotationalVelocity.Z = vel.Z; + } } if (_zeroFlag) { - m_lastposition = _position; - m_lastorientation = _orientation; - - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; + if (lastZeroFlag) + { + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + } if (!m_lastUpdateSent) { - m_throttleUpdates = false; - throttleCounter = 0; - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; + if (lastZeroFlag) + m_lastUpdateSent = true; } + return; } - else - { - if (lastZeroFlag != _zeroFlag) - { - base.RequestPhysicsterseUpdate(); - } - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) - { - m_lastposition = _position; - m_lastorientation = _orientation; - m_lastVelocity = _velocity; - base.RequestPhysicsterseUpdate(); - } - else - { - throttleCounter++; - } - } - } - 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; + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; - _zeroFlag = true; - - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; - } + _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 72ac605d15..1d9fa93c5e 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); - // Move characters - lock (_characters) + // 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(); + } + } + } + */ + + 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); From 5a8fdc8a0b79c14382872571b113b5c5559083c4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 23 Apr 2012 20:16:53 +0100 Subject: [PATCH 7/7] ubitODE - do own timing control (as chODE does) until heartbeat does it right --- .../Region/Physics/UbitOdePlugin/ODEPrim.cs | 8 ++-- .../Region/Physics/UbitOdePlugin/OdeScene.cs | 47 ++++++++++++------- 2 files changed, 34 insertions(+), 21 deletions(-) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 7c0bbef99c..dc6c18d220 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 1d9fa93c5e..cf74f14a60 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; } }