From 7980a1d8496b71e46eeead7d77382cd10ac9fa78 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 2 Jan 2013 19:39:46 +0000 Subject: [PATCH] *TEST* avatar unscripted sit. Some guessing/automation --- .../Region/Framework/Scenes/ScenePresence.cs | 6 +- .../Region/Physics/Manager/PhysicsScene.cs | 21 +- .../Physics/UbitOdePlugin/ODECharacter.cs | 13 + .../UbitOdePlugin/ODERayCastRequestManager.cs | 595 +++++++++++------- .../Physics/UbitOdePlugin/ODESitAvatar.cs | 258 ++++++-- .../Region/Physics/UbitOdePlugin/OdeScene.cs | 276 +++++--- 6 files changed, 821 insertions(+), 348 deletions(-) diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs index 85f4ee59e6..2a39ffd99e 100644 --- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs +++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs @@ -2280,8 +2280,10 @@ namespace OpenSim.Region.Framework.Scenes ParentPart = part; ParentID = part.LocalId; - - Animator.TrySetMovementAnimation("SIT"); + if(status == 3) + Animator.TrySetMovementAnimation("SIT_GROUND"); + else + Animator.TrySetMovementAnimation("SIT"); SendAvatarDataToAllAgents(); part.ParentGroup.TriggerScriptChangedEvent(Changed.LINK); diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 86e07133a8..d24ab2ac73 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -38,6 +38,9 @@ namespace OpenSim.Region.Physics.Manager public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal); public delegate void RayCallback(List list); + public delegate void ProbeBoxCallback(List list); + public delegate void ProbeSphereCallback(List list); + public delegate void ProbePlaneCallback(List list); public delegate void SitAvatarCallback(int status, uint partID, Vector3 offset, Quaternion Orientation); public delegate void JointMoved(PhysicsJoint joint); @@ -56,6 +59,7 @@ namespace OpenSim.Region.Physics.Manager volumedtc = 0x40, // ray cast colision control (may only work for meshs) + ContactsUnImportant = 0x2000, BackFaceCull = 0x4000, ClosestHit = 0x8000, @@ -351,13 +355,26 @@ namespace OpenSim.Region.Physics.Manager return false; } - public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){} - public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { } public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) { return new List(); } + public virtual List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) + { + return new List(); + } + + public virtual List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) + { + return new List(); + } + + public virtual List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) + { + return new List(); + } + public virtual int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) { return 0; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index bb04ea73be..e1d694ecb5 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -715,7 +715,17 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 off = _velocity; float t = 0.5f * timeStep; off = off * t; + d.Quaternion qtmp; + d.GeomCopyQuaternion(bbox, out qtmp); + Quaternion q; + q.X = qtmp.X; + q.Y = qtmp.Y; + q.Z = qtmp.Z; + q.W = qtmp.W; + off *= Quaternion.Conjugate(q); + d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z); + off.X = 2.0f * (m_size.X + Math.Abs(off.X)); off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y)); off.Z = m_size.Z + 2.0f * Math.Abs(off.Z); @@ -741,6 +751,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); } + uint cat1 = d.GeomGetCategoryBits(bbox); + uint col1 = d.GeomGetCollideBits(bbox); + } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 54a83c2de4..31757a9bca 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -56,8 +56,11 @@ namespace OpenSim.Region.Physics.OdePlugin private OdeScene m_scene; IntPtr ray; // the ray. we only need one for our lifetime + IntPtr Sphere; + IntPtr Box; + IntPtr Plane; - private const int ColisionContactGeomsPerTest = 5; + private int CollisionContactGeomsPerTest = 25; private const int DefaultMaxCount = 25; private const int MaxTimePerCallMS = 30; @@ -65,6 +68,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// ODE near callback delegate /// private d.NearCallback nearCallback; + private d.NearCallback nearProbeCallback; private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); private List m_contactResults = new List(); private RayFilterFlags CurrentRayFilter; @@ -74,169 +78,21 @@ namespace OpenSim.Region.Physics.OdePlugin { m_scene = pScene; nearCallback = near; + nearProbeCallback = nearProbe; ray = d.CreateRay(IntPtr.Zero, 1.0f); - d.GeomSetCategoryBits(ray,0); + d.GeomSetCategoryBits(ray, 0); + Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f); + d.GeomSetCategoryBits(Box, 0); + Sphere = d.CreateSphere(IntPtr.Zero,1.0f); + d.GeomSetCategoryBits(Sphere, 0); + Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f); + d.GeomSetCategoryBits(Sphere, 0); } - /// - /// Queues request for a raycast to all world - /// - /// Origin of Ray - /// Ray direction - /// Ray length - /// Return method to send the results - public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod) + public void QueueRequest(ODERayRequest req) { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.Count = DefaultMaxCount; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - /// - /// Queues request for a raycast to particular part - /// - /// Origin of Ray - /// Ray direction - /// Ray length - /// Return method to send the results - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = DefaultMaxCount; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.Count = DefaultMaxCount; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = DefaultMaxCount; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - /// - /// Queues a raycast - /// - /// Origin of Ray - /// Ray normal - /// Ray length - /// - /// Return method to send the results - public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - - public void QueueRequest(Vector3 position, Vector3 direction, float length, int count,RayFilterFlags filter , RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = filter; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count,RayFilterFlags flags, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = flags; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = RayFilterFlags.AllPrims; + if (req.Count == 0) + req.Count = DefaultMaxCount; m_PendingRequests.Enqueue(req); } @@ -272,21 +128,64 @@ namespace OpenSim.Region.Physics.OdePlugin CurrentRayFilter = req.filter; CurrentMaxCount = req.Count; + CollisionContactGeomsPerTest = req.Count & 0xffff; + closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); - d.GeomRaySetLength(ray, req.length); - d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); - d.GeomRaySetParams(ray, 0, backfacecull); - d.GeomRaySetClosestHit(ray, closestHit); + if (req.callbackMethod is ProbeBoxCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z); + d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z); + d.Quaternion qtmp; + qtmp.X = req.orientation.X; + qtmp.Y = req.orientation.Y; + qtmp.Z = req.orientation.Z; + qtmp.W = req.orientation.W; + d.GeomSetOffsetWorldQuaternion(Box, ref qtmp); + } + else if (req.callbackMethod is ProbeSphereCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; - if (req.callbackMethod is RaycastCallback) - // if we only want one get only one per colision pair saving memory - CurrentRayFilter |= RayFilterFlags.ClosestHit; + d.GeomSphereSetRadius(Sphere, req.length); + d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z); + } + else if (req.callbackMethod is ProbePlaneCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + + d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length); + } + + else + { + if (CollisionContactGeomsPerTest > 25) + CollisionContactGeomsPerTest = 25; + + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + d.GeomRaySetParams(ray, 0, backfacecull); + d.GeomRaySetClosestHit(ray, closestHit); + + if (req.callbackMethod is RaycastCallback) + // if we only want one get only one per Collision pair saving memory + CurrentRayFilter |= RayFilterFlags.ClosestHit; + } + + if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) + unchecked + { + CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT; + } if (req.geom == IntPtr.Zero) { - // translate ray filter to colision flags + // translate ray filter to Collision flags catflags = 0; if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0) catflags |= CollisionCategories.VolumeDtc; @@ -303,15 +202,48 @@ namespace OpenSim.Region.Physics.OdePlugin if (catflags != 0) { - d.GeomSetCollideBits(ray, (uint)catflags); - doSpaceRay(req); + if (req.callbackMethod is ProbeBoxCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Box, (uint)catflags); + d.GeomSetCategoryBits(Box, (uint)catflags); + doProbe(req, Box); + } + else if (req.callbackMethod is ProbeSphereCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Sphere, (uint)catflags); + d.GeomSetCategoryBits(Sphere, (uint)catflags); + doProbe(req, Sphere); + } + else if (req.callbackMethod is ProbePlaneCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Plane, (uint)catflags); + d.GeomSetCategoryBits(Plane, (uint)catflags); + doPlane(req); + } + else + { + d.GeomSetCollideBits(ray, (uint)catflags); + doSpaceRay(req); + } } } else { // if we select a geom don't use filters - d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); - doGeomRay(req); + + if (req.callbackMethod is ProbePlaneCallback) + { + d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); + doPlane(req); + } + else + { + d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); + doGeomRay(req); + } } } @@ -396,6 +328,61 @@ namespace OpenSim.Region.Physics.OdePlugin } } + private void doProbe(ODERayRequest req, IntPtr probe) + { + // Collide tests + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + if (req.callbackMethod is ProbeBoxCallback) + ((ProbeBoxCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeSphereCallback) + ((ProbeSphereCallback)req.callbackMethod)(cresult); + } + + private void doPlane(ODERayRequest req) + { + // Collide tests + if (req.geom == IntPtr.Zero) + { + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + } + else + { + d.SpaceCollide2(Plane, req.geom, IntPtr.Zero, nearCallback); + } + + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + + ((ProbePlaneCallback)req.callbackMethod)(cresult); + } + /// /// Method that actually initiates the raycast with a geom /// @@ -450,7 +437,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) { IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; - if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest) + if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest) return false; IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); @@ -483,7 +470,157 @@ namespace OpenSim.Region.Physics.OdePlugin int count = 0; try { - count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); + return; + } + + if (count == 0) + return; + + uint cat1 = d.GeomGetCategoryBits(g1); + uint cat2 = d.GeomGetCategoryBits(g2); + uint col1 = d.GeomGetCollideBits(g1); + uint col2 = d.GeomGetCollideBits(g2); + + + uint ID = 0; + PhysicsActor p2 = null; + + m_scene.actor_name_map.TryGetValue(g2, out p2); + + if (p2 == null) + return; + + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Prim: + + RayFilterFlags thisFlags; + + if (p2.IsPhysical) + thisFlags = RayFilterFlags.physical; + else + thisFlags = RayFilterFlags.nonphysical; + + if (p2.Phantom) + thisFlags |= RayFilterFlags.phantom; + + if (p2.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; + + if ((thisFlags & CurrentRayFilter) == 0) + return; + + ID = ((OdePrim)p2).LocalID; + break; + + case (int)ActorTypes.Agent: + + if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + return; + else + ID = ((OdeCharacter)p2).LocalID; + break; + + case (int)ActorTypes.Ground: + + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + break; + + case (int)ActorTypes.Water: + + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + break; + + default: + break; + } + + d.ContactGeom curcontact = new d.ContactGeom(); + + // closestHit for now only works for meshs, so must do it for others + if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) + { + // Loop all contacts, build results. + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Depth = curcontact.depth; + collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, + curcontact.normal.Z); + lock (m_contactResults) + { + m_contactResults.Add(collisionresult); + if (m_contactResults.Count >= CurrentMaxCount) + return; + } + } + } + else + { + // keep only closest contact + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Depth = float.MaxValue; + + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + + if (curcontact.depth < collisionresult.Depth) + { + collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Depth = curcontact.depth; + collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, + curcontact.normal.Z); + } + } + + if (collisionresult.Depth != float.MaxValue) + { + lock (m_contactResults) + m_contactResults.Add(collisionresult); + } + } + } + + private void nearProbe(IntPtr space, IntPtr g1, IntPtr g2) + { + if (g1 == IntPtr.Zero || g1 == g2) + return; + + if (m_contactResults.Count >= CurrentMaxCount) + return; + + if (d.GeomIsSpace(g1)) + { + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearProbeCallback); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); + } + return; + } + + int count = 0; + try + { + count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); } catch (Exception e) { @@ -495,82 +632,58 @@ namespace OpenSim.Region.Physics.OdePlugin return; uint ID = 0; - PhysicsActor p2 = null; + PhysicsActor p1 = null; - m_scene.actor_name_map.TryGetValue(g2, out p2); + m_scene.actor_name_map.TryGetValue(g1, out p1); - if (p2 == null) - { - /* - string name; - - if (!m_scene.geom_name_map.TryGetValue(g2, out name)) - return; - - if (name == "Terrain") - { - // land colision - if ((CurrentRayFilter & RayFilterFlags.land) == 0) - return; - } - else if (name == "Water") - { - if ((CurrentRayFilter & RayFilterFlags.water) == 0) - return; - } - else - return; - */ + if (p1 == null) return; - } - else + + switch (p1.PhysicsActorType) { - switch (p2.PhysicsActorType) - { - case (int)ActorTypes.Prim: + case (int)ActorTypes.Prim: - RayFilterFlags thisFlags; + RayFilterFlags thisFlags; - if (p2.IsPhysical) - thisFlags = RayFilterFlags.physical; - else - thisFlags = RayFilterFlags.nonphysical; + if (p1.IsPhysical) + thisFlags = RayFilterFlags.physical; + else + thisFlags = RayFilterFlags.nonphysical; - if (p2.Phantom) - thisFlags |= RayFilterFlags.phantom; + if (p1.Phantom) + thisFlags |= RayFilterFlags.phantom; - if (p2.IsVolumeDtc) - thisFlags |= RayFilterFlags.volumedtc; + if (p1.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; - if ((thisFlags & CurrentRayFilter) == 0) - return; + if ((thisFlags & CurrentRayFilter) == 0) + return; - ID = ((OdePrim)p2).LocalID; - break; + ID = ((OdePrim)p1).LocalID; + break; - case (int)ActorTypes.Agent: + case (int)ActorTypes.Agent: - if ((CurrentRayFilter & RayFilterFlags.agent) == 0) - return; - else - ID = ((OdeCharacter)p2).LocalID; - break; + if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + return; + else + ID = ((OdeCharacter)p1).LocalID; + break; - case (int)ActorTypes.Ground: + case (int)ActorTypes.Ground: - if ((CurrentRayFilter & RayFilterFlags.land) == 0) - return; - break; + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + break; - case (int)ActorTypes.Water: + case (int)ActorTypes.Water: - if ((CurrentRayFilter & RayFilterFlags.water) == 0) - return; - break; + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + break; - default: - break; - } + default: + break; } d.ContactGeom curcontact = new d.ContactGeom(); @@ -638,6 +751,21 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomDestroy(ray); ray = IntPtr.Zero; } + if (Box != IntPtr.Zero) + { + d.GeomDestroy(Box); + Box = IntPtr.Zero; + } + if (Sphere != IntPtr.Zero) + { + d.GeomDestroy(Sphere); + Sphere = IntPtr.Zero; + } + if (Plane != IntPtr.Zero) + { + d.GeomDestroy(Plane); + Plane = IntPtr.Zero; + } } } @@ -650,5 +778,6 @@ namespace OpenSim.Region.Physics.OdePlugin public float length; public object callbackMethod; public RayFilterFlags filter; + public Quaternion orientation; } } \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs index fd3a3ba8ce..9e237635fb 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -54,6 +54,20 @@ namespace OpenSim.Region.Physics.OdePlugin private static Vector3 SitAjust = new Vector3(0, 0, 0.4f); private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit; + private void RotAroundZ(float x, float y, ref Quaternion ori) + { + double ang = Math.Atan2(y, x); + ang *= 0.5d; + float s = (float)Math.Sin(ang); + float c = (float)Math.Cos(ang); + + ori.X = 0; + ori.Y = 0; + ori.Z = s; + ori.W = c; + } + + public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) { if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero) @@ -72,7 +86,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.AABB aabb; - Quaternion ori; + Quaternion ori = Quaternion.Identity; d.Quaternion qtmp; d.GeomCopyQuaternion(geom, out qtmp); Quaternion geomOri; @@ -86,9 +100,14 @@ namespace OpenSim.Region.Physics.OdePlugin geomInvOri.Z = -qtmp.Z; geomInvOri.W = qtmp.W; - Vector3 target = geopos + offset; - Vector3 rayDir = target - avCameraPosition; + Vector3 rayDir = geopos + offset - avCameraPosition; float raylen = rayDir.Length(); + if (raylen < 0.001f) + { + PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); + return; + } + float t = 1 / raylen; rayDir.X *= t; rayDir.Y *= t; @@ -98,9 +117,9 @@ namespace OpenSim.Region.Physics.OdePlugin List rayResults; rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); - if (rayResults.Count == 0 || rayResults[0].ConsumerID != actor.LocalID) + if (rayResults.Count == 0) { - d.GeomGetAABB(geom,out aabb); + d.GeomGetAABB(geom, out aabb); offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); ori = geomInvOri; offset *= geomInvOri; @@ -109,44 +128,45 @@ namespace OpenSim.Region.Physics.OdePlugin return; } - offset = rayResults[0].Pos - geopos; - double ang; - float s; - float c; d.GeomClassID geoclass = d.GeomGetClass(geom); if (geoclass == d.GeomClassID.SphereClass) { - float r = d.GeomSphereGetRadius(geom); + int status = 1; + float r = d.GeomSphereGetRadius(geom); offset.Normalize(); offset *= r; - ang = Math.Atan2(offset.Y, offset.X); - ang *= 0.5d; - s = (float)Math.Sin(ang); - c = (float)Math.Cos(ang); - - ori = new Quaternion(0, 0, s, c); + RotAroundZ(offset.X, offset.Y, ref ori); if (r < 0.4f) { offset = new Vector3(0, 0, r); } - else if (offset.Z < 0.4f) + else { - t = offset.Z; - float rsq = r * r; + if (offset.Z < 0.4f) + { + t = offset.Z; + float rsq = r * r; - t = 1.0f / (rsq - t * t); - offset.X *= t; - offset.Y *= t; - offset.Z = 0.4f; - t = rsq - 0.16f; - offset.X *= t; - offset.Y *= t; + t = 1.0f / (rsq - t * t); + offset.X *= t; + offset.Y *= t; + offset.Z = 0.4f; + t = rsq - 0.16f; + offset.X *= t; + offset.Y *= t; + } + else if (r > 0.8f && offset.Z > 0.8f * r) + { + status = 3; + avOffset.X = -avOffset.X; + avOffset.Z += 0.4f; + } } offset += avOffset * ori; @@ -154,27 +174,189 @@ namespace OpenSim.Region.Physics.OdePlugin ori = geomInvOri * ori; offset *= geomInvOri; - PhysicsSitResponse(1, actor.LocalID, offset, ori); + PhysicsSitResponse(status, actor.LocalID, offset, ori); return; } -/* - // contact normals aren't reliable on meshs or sculpts it seems - Vector3 norm = rayResults[0].Normal; + Vector3 norm = rayResults[0].Normal; - if (norm.Z < 0) + if (norm.Z < -0.4f) + { + PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); + return; + } + + float SitNormX = -rayDir.X; + float SitNormY = -rayDir.Y; + + Vector3 pivot = geopos + offset; + + float edgeNormalX = norm.X; + float edgeNormalY = norm.Y; + float edgeDirX = -rayDir.X; + float edgeDirY = -rayDir.Y; + Vector3 edgePos = rayResults[0].Pos; + float edgeDist = float.MaxValue; + + bool foundEdge = false; + + if (norm.Z < 0.5f) + { + float rayDist = 4.0f; + float curEdgeDist = 0.0f; + pivot = geopos + offset; + + for (int i = 0; i < 6; i++) { - PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); + pivot.X -= 0.005f * norm.X; + pivot.Y -= 0.005f * norm.Y; + pivot.Z -= 0.005f * norm.Z; + + rayDir.X = -norm.X * norm.Z; + rayDir.Y = -norm.Y * norm.Z; + rayDir.Z = 1.0f - norm.Z * norm.Z; + rayDir.Normalize(); + + rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); + if (rayResults.Count == 0) + break; + + curEdgeDist += rayResults[0].Depth; + + if (Math.Abs(rayResults[0].Normal.Z) < 0.7f) + { + rayDist -= rayResults[0].Depth; + if (rayDist < 0f) + break; + + pivot = rayResults[0].Pos; + norm = rayResults[0].Normal; + edgeNormalX = norm.X; + edgeNormalY = norm.Y; + edgeDirX = rayDir.X; + edgeDirY = rayDir.Y; + } + else + { + foundEdge = true; + if (curEdgeDist < edgeDist) + { + edgeDist = curEdgeDist; + edgePos = rayResults[0].Pos; + } + break; + } + } + + if (!foundEdge) + { + PhysicsSitResponse(0, actor.LocalID, offset, ori); return; } -*/ + avOffset.X *= 0.5f; + } - ang = Math.Atan2(-rayDir.Y, -rayDir.X); - ang *= 0.5d; - s = (float)Math.Sin(ang); - c = (float)Math.Cos(ang); + else if (norm.Z > 0.866f) + { + float toCamBaseX = avCameraPosition.X - pivot.X; + float toCamBaseY = avCameraPosition.Y - pivot.Y; + float toCamX = toCamBaseX; + float toCamY = toCamBaseY; - ori = new Quaternion(0, 0, s, c); + for (int j = 0; j < 4; j++) + { + float rayDist = 1.0f; + float curEdgeDist = 0.0f; + pivot = geopos + offset; + + for (int i = 0; i < 3; i++) + { + pivot.Z -= 0.005f; + rayDir.X = toCamX; + rayDir.Y = toCamY; + rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z; + rayDir.Normalize(); + + rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); + if (rayResults.Count == 0) + break; + + curEdgeDist += rayResults[0].Depth; + + if (rayResults[0].Normal.Z > 0.5f) + { + rayDist -= rayResults[0].Depth; + if (rayDist < 0f) + break; + + pivot = rayResults[0].Pos; + norm = rayResults[0].Normal; + } + else + { + foundEdge = true; + if (curEdgeDist < edgeDist) + { + edgeDist = curEdgeDist; + edgeNormalX = rayResults[0].Normal.X; + edgeNormalY = rayResults[0].Normal.Y; + edgeDirX = rayDir.X; + edgeDirY = rayDir.Y; + edgePos = rayResults[0].Pos; + } + break; + } + } + if (foundEdge && edgeDist < 0.2f) + break; + + switch (j) + { + case 0: + toCamX = -toCamBaseY; + toCamY = toCamBaseX; + break; + case 1: + toCamX = toCamBaseY; + toCamY = -toCamBaseX; + break; + case 2: + toCamX = -toCamBaseX; + toCamY = -toCamBaseY; + break; + default: + break; + } + } + + if (!foundEdge) + { + avOffset.X = -avOffset.X; + avOffset.Z += 0.4f; + + RotAroundZ(SitNormX, SitNormY, ref ori); + + offset += avOffset * ori; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(3, actor.LocalID, offset, ori); + return; + } + avOffset.X *= 0.5f; + } + + SitNormX = edgeNormalX; + SitNormY = edgeNormalY; + offset = edgePos - geopos; + if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0) + { + SitNormX = -SitNormX; + SitNormY = -SitNormY; + } + + RotAroundZ(SitNormX, SitNormY, ref ori); offset += avOffset * ori; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index d344d4d6a4..d045b59484 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2579,7 +2579,16 @@ namespace OpenSim.Region.Physics.OdePlugin { if (retMethod != null) { - m_rayCastManager.QueueRequest(position, direction, length, retMethod); + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + req.filter = RayFilterFlags.All; + + m_rayCastManager.QueueRequest(req); } } @@ -2587,29 +2596,51 @@ namespace OpenSim.Region.Physics.OdePlugin { if (retMethod != null) { - m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = RayFilterFlags.All; + + m_rayCastManager.QueueRequest(req); } } - // don't like this + public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) { - ContactResult[] ourResults = null; + List ourresults = new List(); + object SyncObject = new object(); + RayCallback retMethod = delegate(List results) { - ourResults = new ContactResult[results.Count]; - results.CopyTo(ourResults, 0); + lock (SyncObject) + { + ourresults = results; + Monitor.PulseAll(SyncObject); + } }; - int waitTime = 0; - m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); - while (ourResults == null && waitTime < 1000) + + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = RayFilterFlags.All; + + lock (SyncObject) { - Thread.Sleep(1); - waitTime++; + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return null; + else + return ourresults; } - if (ourResults == null) - return new List(); - return new List(ourResults); } public override bool SuportsRaycastWorldFiltered() @@ -2631,9 +2662,18 @@ namespace OpenSim.Region.Physics.OdePlugin } }; + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = filter; + lock (SyncObject) { - m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod); + m_rayCastManager.QueueRequest(req); if (!Monitor.Wait(SyncObject, 500)) return null; else @@ -2641,73 +2681,163 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) - { - if (retMethod != null && actor !=null) - { - IntPtr geom; - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - else - return; - if (geom == IntPtr.Zero) - return; - m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod); - } - } - - public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) - { - if (retMethod != null && actor != null) - { - IntPtr geom; - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - else - return; - if (geom == IntPtr.Zero) - return; - - m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); - } - } - public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) { + if (actor == null) + return new List(); + + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return new List(); + + if (geom == IntPtr.Zero) + return new List(); + + List ourResults = null; + object SyncObject = new object(); + + RayCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) + { + List ourResults = null; + object SyncObject = new object(); + + ProbeBoxCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.Normal = size; + req.Origin = position; + req.orientation = orientation; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) + { + List ourResults = null; + object SyncObject = new object(); + + ProbeSphereCallback retMethod = delegate(List results) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + }; + + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = radius; + req.Origin = position; + req.Count = Count; + req.filter = flags; + + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) + { + IntPtr geom = IntPtr.Zero;; + if (actor != null) { - IntPtr geom; if (actor is OdePrim) geom = ((OdePrim)actor).prim_geom; else if (actor is OdeCharacter) geom = ((OdePrim)actor).prim_geom; - else - return new List(); - if (geom == IntPtr.Zero) - return new List(); - - ContactResult[] ourResults = null; - RayCallback retMethod = delegate(List results) - { - ourResults = new ContactResult[results.Count]; - results.CopyTo(ourResults, 0); - }; - int waitTime = 0; - m_rayCastManager.QueueRequest(geom,position, direction, length, Count, flags, retMethod); - while (ourResults == null && waitTime < 1000) - { - Thread.Sleep(1); - waitTime++; - } - if (ourResults == null) - return new List(); - return new List(ourResults); } - return new List(); + + List ourResults = null; + object SyncObject = new object(); + + ProbePlaneCallback retMethod = delegate(List results) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + }; + + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = plane.W; + req.Normal.X = plane.X; + req.Normal.Y = plane.Y; + req.Normal.Z = plane.Z; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; } public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)