From 80639ace95089414423b16f90c8994f50e010373 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 11 Dec 2012 04:36:27 +0000 Subject: [PATCH] a few more changes on avatar collider --- .../Physics/UbitOdePlugin/ODECharacter.cs | 345 ++++++++++++++---- .../Region/Physics/UbitOdePlugin/OdeScene.cs | 345 ++++++------------ 2 files changed, 377 insertions(+), 313 deletions(-) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 15bdc5705f..1b25faf16f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -74,7 +74,6 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 _position; private Vector3 _zeroPosition; - private bool _zeroFlag = false; private Vector3 _velocity; private Vector3 _target_velocity; private Vector3 _acceleration; @@ -90,11 +89,15 @@ namespace OpenSim.Region.Physics.OdePlugin public float PID_D; public float PID_P; + private float timeStep; + private float invtimeStep; + private float m_feetOffset = 0; private float feetOff = 0; private float feetSZ = 0.5f; const float feetScale = 0.8f; private float boneOff = 0; + private float m_lastVelocitySqr = 0; public float walkDivisor = 1.3f; public float runDivisor = 0.8f; @@ -103,6 +106,9 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_iscollidingGround = false; private bool m_iscollidingObj = false; private bool m_alwaysRun = false; + + private bool _zeroFlag = false; + private int m_requestedUpdateFrequency = 0; private uint m_localID = 0; public bool m_returnCollisions = false; @@ -120,6 +126,7 @@ namespace OpenSim.Region.Physics.OdePlugin int m_colliderfilter = 0; int m_colliderGroundfilter = 0; int m_colliderObjectfilter = 0; + bool m_collisionException = false; // Default we're a Character private CollisionCategories m_collisionCategories = (CollisionCategories.Character); @@ -132,10 +139,11 @@ namespace OpenSim.Region.Physics.OdePlugin // we do land collisions not ode | CollisionCategories.Land); public IntPtr Body = IntPtr.Zero; private OdeScene _parent_scene; - public IntPtr topbox = IntPtr.Zero; - public IntPtr midbox = IntPtr.Zero; - public IntPtr feetbox = IntPtr.Zero; - public IntPtr bonebox = IntPtr.Zero; + private IntPtr topbox = IntPtr.Zero; + private IntPtr midbox = IntPtr.Zero; + private IntPtr feetbox = IntPtr.Zero; + private IntPtr bbox = IntPtr.Zero; + public IntPtr collider = IntPtr.Zero; public IntPtr Amotor = IntPtr.Zero; @@ -143,6 +151,7 @@ namespace OpenSim.Region.Physics.OdePlugin + public int m_eventsubscription = 0; private int m_cureventsubscription = 0; private CollisionEventUpdate CollisionEventsThisFrame = null; @@ -160,6 +169,9 @@ namespace OpenSim.Region.Physics.OdePlugin { m_uuid = UUID.Random(); + timeStep = parent_scene.ODE_STEPSIZE; + invtimeStep = 1 / timeStep; + if (pos.IsFinite()) { if (pos.Z > 99999f) @@ -208,8 +220,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default - PID_D = basePID_D * m_mass / parent_scene.ODE_STEPSIZE; - PID_P = basePID_P * m_mass / parent_scene.ODE_STEPSIZE; + PID_D = basePID_D * m_mass * invtimeStep; + PID_P = basePID_P * m_mass * invtimeStep; m_isPhysical = false; // current status: no ODE information exists @@ -292,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin set { flying = value; - // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); +// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); } } @@ -336,25 +348,25 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_iscollidingGround; } set { - /* we now control this - if (value) - { - m_colliderGroundfilter += 2; - if (m_colliderGroundfilter > 2) - m_colliderGroundfilter = 2; - } - else - { - m_colliderGroundfilter--; - if (m_colliderGroundfilter < 0) - m_colliderGroundfilter = 0; - } +/* we now control this + if (value) + { + m_colliderGroundfilter += 2; + if (m_colliderGroundfilter > 2) + m_colliderGroundfilter = 2; + } + else + { + m_colliderGroundfilter--; + if (m_colliderGroundfilter < 0) + m_colliderGroundfilter = 0; + } - if (m_colliderGroundfilter == 0) - m_iscollidingGround = false; - else - m_iscollidingGround = true; - */ + if (m_colliderGroundfilter == 0) + m_iscollidingGround = false; + else + m_iscollidingGround = true; + */ } } @@ -386,7 +398,7 @@ namespace OpenSim.Region.Physics.OdePlugin else m_iscollidingObj = true; - // m_iscollidingObj = value; +// m_iscollidingObj = value; if (m_iscollidingObj) m_pidControllerActive = false; @@ -634,8 +646,8 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_orientation; } set { - // fakeori = value; - // givefakeori++; +// fakeori = value; +// givefakeori++; value.Normalize(); AddChange(changes.Orientation, value); @@ -690,6 +702,46 @@ namespace OpenSim.Region.Physics.OdePlugin AddChange(changes.Momentum, momentum); } + private void ajustCollider() + { + float vq = _velocity.LengthSquared(); + if (m_lastVelocitySqr != vq) + { + m_lastVelocitySqr = vq; + if (vq > 100.0f) + { + Vector3 off = _velocity; + float t = 0.5f * timeStep; + off = off * t; + 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); + d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z); + + d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(bbox, (uint)m_collisionFlags); + d.GeomSetCategoryBits(topbox, 0); + d.GeomSetCollideBits(topbox, 0); + d.GeomSetCategoryBits(midbox, 0); + d.GeomSetCollideBits(midbox, 0); + d.GeomSetCategoryBits(feetbox, 0); + d.GeomSetCollideBits(feetbox, 0); + } + else + { + d.GeomSetCategoryBits(bbox, 0); + d.GeomSetCollideBits(bbox, 0); + d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); + d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); + d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); + } + } + } + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) { // sizes one day should came from visual parameters @@ -697,7 +749,6 @@ namespace OpenSim.Region.Physics.OdePlugin float sy = m_size.Y; float sz = m_size.Z; - float topsx = sx * 0.9f; float midsx = sx; float feetsx = sx * feetScale; @@ -732,21 +783,17 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); - feetbox = d.CreateBox(_parent_scene.CharsSpace, feetsx, feetsy, feetsz); - d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); + collider = d.HashSpaceCreate(_parent_scene.CharsSpace); + d.HashSpaceSetLevels(collider, -4, 3); + d.SpaceSetSublevel(collider, 3); + d.SpaceSetCleanup(collider, false); + d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); + d.GeomSetCollideBits(collider, (uint)m_collisionFlags); - midbox = d.CreateBox(_parent_scene.CharsSpace, midsx, midsy, midsz); - d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); - - topbox = d.CreateBox(_parent_scene.CharsSpace, topsx, topsy, topsz); - d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); - - bonebox = d.CreateBox(_parent_scene.CharsSpace, bonesx, bonesy, bonesz); - d.GeomSetCategoryBits(bonebox, (uint)m_collisionCategories); - d.GeomSetCollideBits(bonebox, (uint)m_collisionFlags); + feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz); + midbox = d.CreateBox(collider, midsx, midsy, midsz); + topbox = d.CreateBox(collider, topsx, topsy, topsz); + bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z); m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass @@ -758,9 +805,13 @@ namespace OpenSim.Region.Physics.OdePlugin Body = d.BodyCreate(_parent_scene.world); _zeroFlag = false; + m_collisionException = false; m_pidControllerActive = true; m_freemove = false; + _velocity = Vector3.Zero; + m_lastVelocitySqr = 0; + d.BodySetAutoDisableFlag(Body, false); d.BodySetPosition(Body, npositionX, npositionY, npositionZ); @@ -772,12 +823,14 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetBody(feetbox, Body); d.GeomSetBody(midbox, Body); d.GeomSetBody(topbox, Body); - d.GeomSetBody(bonebox, Body); + d.GeomSetBody(bbox, Body); d.GeomSetOffsetPosition(feetbox, 0, 0, feetz); d.GeomSetOffsetPosition(midbox, 0, 0, midz); d.GeomSetOffsetPosition(topbox, 0, 0, topz); - d.GeomSetOffsetPosition(bonebox, 0, 0, m_feetOffset); + + ajustCollider(); + // The purpose of the AMotor here is to keep the avatar's physical // surrogate from rotating while moving @@ -841,44 +894,109 @@ namespace OpenSim.Region.Physics.OdePlugin if (topbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(topbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); + _parent_scene.waitForSpaceUnlock(collider); d.GeomDestroy(topbox); topbox = IntPtr.Zero; } if (midbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(midbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); + _parent_scene.waitForSpaceUnlock(collider); d.GeomDestroy(midbox); midbox = IntPtr.Zero; } if (feetbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(feetbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); + _parent_scene.waitForSpaceUnlock(collider); d.GeomDestroy(feetbox); feetbox = IntPtr.Zero; } - if (bonebox != IntPtr.Zero) + if (bbox != IntPtr.Zero) { - _parent_scene.actor_name_map.Remove(bonebox); - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); - d.GeomDestroy(bonebox); - bonebox = IntPtr.Zero; + _parent_scene.actor_name_map.Remove(bbox); + _parent_scene.waitForSpaceUnlock(collider); + d.GeomDestroy(bbox); + bbox = IntPtr.Zero; + } + + if (collider != IntPtr.Zero) + { + d.SpaceDestroy(collider); + collider = IntPtr.Zero; } } + //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z + public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) + { + float sin = 2.0f * rot.Z * rot.W; + float cos = rot.W * rot.W - rot.Z * rot.Z; + float tx = x; + + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) + { + float tx = x; + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) + { + float tx = x; + x = tx * cos + y * sin; + y = -tx * sin + y * cos; + } + + public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) + { + float sin = - 2.0f * rot.Z * rot.W; + float cos = rot.W * rot.W - rot.Z * rot.Z; + float tx = x; + + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + + public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) { feetcollision = false; + if (m_collisionException) + return false; - if (me == bonebox) // inner bone + if (me == bbox) // if moving fast { - if (contact.pos.Z - _position.Z < boneOff) - IsColliding = true; - return true; + // force a full inelastic collision + m_collisionException = true; + + Vector3 off = m_size * 0.5f; + off.X += contact.depth; + off.Y += contact.depth; + off.Z += contact.depth; + if (reverse) + { + off.X *= -contact.normal.X; + off.Y *= -contact.normal.Y; + off.Z *= -contact.normal.Z; + } + else + { + off.X *= contact.normal.X; + off.Y *= contact.normal.Y; + off.Z *= contact.normal.Z; + } + + off.X += contact.pos.X; + off.Y += contact.pos.Y; + off.Z += contact.pos.Z; + + _position = off; + return false; } if (me == topbox) // keep a box head @@ -890,6 +1008,21 @@ namespace OpenSim.Region.Physics.OdePlugin if (me == midbox) { + if (Math.Abs(contact.normal.Z) > 0.95f) + { + float nz = contact.normal.Z; + if (!reverse) + nz = -nz; + + if (nz > 0) + return true; // missed head TODO + + // missed feet collision? + + + return true; + } + t = offx * offx + offy * offy; t = (float)Math.Sqrt(t); t = 1 / t; @@ -917,12 +1050,19 @@ namespace OpenSim.Region.Physics.OdePlugin if (Math.Abs(contact.normal.Z) > 0.95f) { + if (contact.normal.Z > 0) + contact.normal.Z = 1.0f; + else + contact.normal.Z = -1.0f; + contact.normal.X = 0.0f; + contact.normal.Y = 0.0f; feetcollision = true; if (h < boneOff) IsColliding = true; return true; } + float offz = h - feetOff; // distance from top of feetbox if (offz > 0) @@ -971,11 +1111,28 @@ namespace OpenSim.Region.Physics.OdePlugin /// This is the avatar's movement control + PID Controller /// /// - public void Move(float timeStep, List defects) + public void Move(List defects) { if (Body == IntPtr.Zero) return; + if (m_collisionException) + { + d.BodySetPosition(Body,_position.X, _position.Y, _position.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + + float v = _velocity.Length(); + if (v != 0) + { + v = 6.0f / v; + _velocity = _velocity * v; + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } + ajustCollider(); + m_collisionException = false; + return; + } + d.Vector3 dtmp = d.BodyGetPosition(Body); Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); @@ -1049,6 +1206,7 @@ namespace OpenSim.Region.Physics.OdePlugin //****************************************** // colide with land + d.AABB aabb; d.GeomGetAABB(feetbox, out aabb); float chrminZ = aabb.MinZ - 0.02f; // move up a bit @@ -1095,8 +1253,8 @@ namespace OpenSim.Region.Physics.OdePlugin contact.Position.X = localpos.X; contact.Position.Y = localpos.Y; contact.Position.Z = chrminZ; - contact.SurfaceNormal.X = 0f; - contact.SurfaceNormal.Y = 0f; + contact.SurfaceNormal.X = 0.0f; + contact.SurfaceNormal.Y = 0.0f; contact.SurfaceNormal.Z = -1f; contact.RelativeSpeed = -vel.Z; contact.CharacterFeet = true; @@ -1118,6 +1276,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_iscollidingGround = false; } + //****************************************** bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); @@ -1253,21 +1412,58 @@ namespace OpenSim.Region.Physics.OdePlugin } // update our local ideia of position velocity and aceleration + // _position = localpos; _position = localpos; + if (_zeroFlag) { _velocity = Vector3.Zero; _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; } else { - _acceleration = _velocity; // previus velocity - _velocity = vel; - _acceleration = (vel - _acceleration) / timeStep; + Vector3 a =_velocity; // previus velocity + SetSmooth(ref _velocity, ref vel, 2); + a = (_velocity - a) * invtimeStep; + SetSmooth(ref _acceleration, ref a, 2); + + dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = 0f; + m_rotationalVelocity.Y = 0f; + m_rotationalVelocity.Z = dtmp.Z; + Math.Round(m_rotationalVelocity.Z,3); } - + ajustCollider(); } + public void round(ref Vector3 v, int digits) + { + v.X = (float)Math.Round(v.X, digits); + v.Y = (float)Math.Round(v.Y, digits); + v.Z = (float)Math.Round(v.Z, digits); + } + + public void SetSmooth(ref Vector3 dst, ref Vector3 value) + { + dst.X = 0.1f * dst.X + 0.9f * value.X; + dst.Y = 0.1f * dst.Y + 0.9f * value.Y; + dst.Z = 0.1f * dst.Z + 0.9f * value.Z; + } + + public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) + { + dst.X = 0.4f * dst.X + 0.6f * value.X; + dst.X = (float)Math.Round(dst.X, rounddigits); + + dst.Y = 0.4f * dst.Y + 0.6f * value.Y; + dst.Y = (float)Math.Round(dst.Y, rounddigits); + + dst.Z = 0.4f * dst.Z + 0.6f * value.Z; + dst.Z = (float)Math.Round(dst.Z, rounddigits); + } + + /// /// Updates the reported position and velocity. /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording @@ -1394,10 +1590,11 @@ namespace OpenSim.Region.Physics.OdePlugin AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); - _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[collider] = (PhysicsActor)this; _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this; + _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; _parent_scene.AddCharacter(this); } else @@ -1450,13 +1647,16 @@ namespace OpenSim.Region.Physics.OdePlugin _position.Z + (m_size.Z - oldsz) * 0.5f); Velocity = Vector3.Zero; + - _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[collider] = (PhysicsActor)this; _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this; + _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; } m_freemove = false; + m_collisionException = false; m_pidControllerActive = true; } else @@ -1565,6 +1765,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); + ajustCollider(); } private void donullchange() @@ -1573,7 +1774,7 @@ namespace OpenSim.Region.Physics.OdePlugin public bool DoAChange(changes what, object arg) { - if (topbox == IntPtr.Zero && what != changes.Add && what != changes.Remove) + if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove) { return false; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 2ba59407f2..f8d7195443 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -178,7 +178,9 @@ namespace OpenSim.Region.Physics.OdePlugin public changes what; public Object arg; } - + + + public class OdeScene : PhysicsScene { private readonly ILog m_log; @@ -301,6 +303,8 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr StaticSpace; // space for the static things around public IntPtr GroundSpace; // space for ground + public IntPtr SharedRay; + // some speedup variables private int spaceGridMaxX; private int spaceGridMaxY; @@ -428,6 +432,8 @@ namespace OpenSim.Region.Physics.OdePlugin contactgroup = d.JointGroupCreate(0); //contactgroup + SharedRay = d.CreateRay(TopSpace, 1.0f); + d.WorldSetAutoDisableFlag(world, false); } } @@ -733,35 +739,35 @@ namespace OpenSim.Region.Physics.OdePlugin if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; -/* -// debug - PhysicsActor dp2; - if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) - { - d.AABB aabb; - d.GeomGetAABB(g2, out aabb); - float x = aabb.MaxX - aabb.MinX; - float y = aabb.MaxY - aabb.MinY; - float z = aabb.MaxZ - aabb.MinZ; - if (x > 60.0f || y > 60.0f || z > 60.0f) - { - if (!actor_name_map.TryGetValue(g2, out dp2)) - m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); - else - m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", - dp2.Name, dp2.Size, x, y, z, - dp2.Position.ToString(), - dp2.Orientation.ToString(), - dp2.Orientation.Length()); - return; - } - } -// -*/ + /* + // debug + PhysicsActor dp2; + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + { + d.AABB aabb; + d.GeomGetAABB(g2, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if (x > 60.0f || y > 60.0f || z > 60.0f) + { + if (!actor_name_map.TryGetValue(g2, out dp2)) + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + else + m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", + dp2.Name, dp2.Size, x, y, z, + dp2.Position.ToString(), + dp2.Orientation.ToString(), + dp2.Orientation.Length()); + return; + } + } + // + */ - if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || - d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) + if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || + d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) { int cflags; unchecked @@ -776,7 +782,7 @@ namespace OpenSim.Region.Physics.OdePlugin catch (SEHException) { m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); -// ode.drelease(world); + // ode.drelease(world); base.TriggerPhysicsBasedRestart(); } catch (Exception e) @@ -816,26 +822,25 @@ namespace OpenSim.Region.Physics.OdePlugin // get first contact d.ContactGeom curContact = new d.ContactGeom(); + if (!GetCurContactGeom(0, ref curContact)) return; - // for now it's the one with max depth - ContactPoint maxDepthContact = new ContactPoint( - new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), - new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), - curContact.depth - ); + // do volume detection case - if ( - (p1.IsVolumeDtc || p2.IsVolumeDtc)) + if ((p1.IsVolumeDtc || p2.IsVolumeDtc)) { + ContactPoint maxDepthContact = new ContactPoint( + new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), + new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), + curContact.depth, false + ); + collision_accounting_events(p1, p2, maxDepthContact); return; } // big messy collision analises - Vector3 normoverride = Vector3.Zero; //damm c# - float mu = 0; float bounce = 0; float cfm = 0.0001f; @@ -846,36 +851,15 @@ namespace OpenSim.Region.Physics.OdePlugin ContactData contactdata1 = new ContactData(0, 0, false); ContactData contactdata2 = new ContactData(0, 0, false); - bool dop1foot = false; - bool dop2foot = false; + bool dop1ava = false; + bool dop2ava = false; bool ignore = false; - bool AvanormOverride = false; switch (p1.PhysicsActorType) { case (int)ActorTypes.Agent: { - dop1foot = true; - - AvanormOverride = true; - Vector3 tmp = p2.Position - p1.Position; - normoverride = p2.Velocity - p1.Velocity; - mu = normoverride.LengthSquared(); - - if (mu > 1e-6) - { - mu = 1.0f / (float)Math.Sqrt(mu); - normoverride *= mu; - mu = Vector3.Dot(tmp, normoverride); - if (mu > 0) - normoverride *= -1; - } - else - { - tmp.Normalize(); - normoverride = -tmp; - } - + dop1ava = true; switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: @@ -886,7 +870,6 @@ namespace OpenSim.Region.Physics.OdePlugin case (int)ActorTypes.Prim: if (p2.Velocity.LengthSquared() > 0.0f) p2.CollidingObj = true; - dop1foot = true; break; default: @@ -901,33 +884,8 @@ namespace OpenSim.Region.Physics.OdePlugin { case (int)ActorTypes.Agent: + dop2ava = true; - dop2foot = true; - - AvanormOverride = true; - - Vector3 tmp = p2.Position - p1.Position; - normoverride = p2.Velocity - p1.Velocity; - mu = normoverride.LengthSquared(); - if (mu > 1e-6) - { - mu = 1.0f / (float)Math.Sqrt(mu); - normoverride *= mu; - mu = Vector3.Dot(tmp, normoverride); - if (mu > 0) - normoverride *= -1; - } - else - { - tmp.Normalize(); - normoverride = -tmp; - } - - bounce = 0; - mu = 0; - cfm = 0.0001f; - - dop2foot = true; if (p1.Velocity.LengthSquared() > 0.0f) p1.CollidingObj = true; break; @@ -1032,170 +990,78 @@ namespace OpenSim.Region.Physics.OdePlugin default: break; } + if (ignore) return; + + d.ContactGeom maxContact = curContact; +// if (IgnoreNegSides && curContact.side1 < 0) +// maxContact.depth = float.MinValue; + + d.ContactGeom minContact = curContact; +// if (IgnoreNegSides && curContact.side1 < 0) +// minContact.depth = float.MaxValue; + IntPtr Joint; - bool FeetCollision = false; - - int i = 0; int ncontacts = 0; - while(true) - { - if (IgnoreNegSides && curContact.side1 < 0) + + int i = 0; + + while (true) { + if (m_global_contactcount >= maxContactsbeforedeath) + break; + +// if (!(IgnoreNegSides && curContact.side1 < 0)) + { + bool noskip = true; + if (dop1ava) + { + if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact, ref FeetCollision))) + + noskip = false; + } + else if (dop2ava) + { + if (!(((OdeCharacter)p2).Collide(g2,true, ref curContact, ref FeetCollision))) + noskip = false; + } + + if (noskip) + { + m_global_contactcount++; + ncontacts++; + + Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); + d.JointAttach(Joint, b1, b2); + + if (curContact.depth > maxContact.depth) + maxContact = curContact; + + if (curContact.depth < minContact.depth) + minContact = curContact; + } + } + if (++i >= count) break; if (!GetCurContactGeom(i, ref curContact)) break; } - else - - { - if(dop1foot) - { - if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision))) - { - if (++i >= count) - break; - else - continue; - } - } - else if(dop2foot) - { - if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision))) - { - if (++i >= count) - break; - else - continue; - } - } - -/* - if (AvanormOverride) - { - if (curContact.depth > 0.3f) - { - if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) - p1.IsColliding = true; - if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) - p2.IsColliding = true; - curContact.normal.X = normoverride.X; - curContact.normal.Y = normoverride.Y; - curContact.normal.Z = normoverride.Z; - } - - else - { - if (dop1foot) - { - float sz = p1.Size.Z; - Vector3 vtmp = p1.Position; - float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; - if (ppos > 0f) - { - if (!p1.Flying) - { - d.AABB aabb; - d.GeomGetAABB(g2, out aabb); - float tmp = vtmp.Z - sz * .18f; - - if (aabb.MaxZ < tmp) - { - vtmp.X = curContact.pos.X - vtmp.X; - vtmp.Y = curContact.pos.Y - vtmp.Y; - vtmp.Z = -0.2f; - vtmp.Normalize(); - curContact.normal.X = vtmp.X; - curContact.normal.Y = vtmp.Y; - curContact.normal.Z = vtmp.Z; - } - } - } - else - p1.IsColliding = true; - - } - - if (dop2foot) - { - float sz = p2.Size.Z; - Vector3 vtmp = p2.Position; - vtmp.Z -= sz * 0.5f; - vtmp.Z += 0.5f; - float ppos = vtmp.Z - curContact.pos.Z; - if (ppos > 0f) - { - if (!p2.Flying) - { - float tmp = vtmp.Z - sz * .18f; - vtmp.X = curContact.pos.X - vtmp.X; - vtmp.Y = curContact.pos.Y - vtmp.Y; - vtmp.Z = curContact.pos.Z - vtmp.Z; - vtmp.Normalize(); - curContact.normal.X = vtmp.X; - curContact.normal.Y = vtmp.Y; - curContact.normal.Z = vtmp.Z; - } - } -// else - p2.IsColliding = true; - - } - } - } -*/ - ncontacts++; - Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); - d.JointAttach(Joint, b1, b2); - - if (++m_global_contactcount >= maxContactsbeforedeath) - break; - - if (++i >= count) - break; - - if (!GetCurContactGeom(i, ref curContact)) - break; - - if (curContact.depth > maxDepthContact.PenetrationDepth) - { - maxDepthContact.Position.X = curContact.pos.X; - maxDepthContact.Position.Y = curContact.pos.Y; - maxDepthContact.Position.Z = curContact.pos.Z; - maxDepthContact.SurfaceNormal.X = curContact.normal.X; - maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; - maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; - maxDepthContact.PenetrationDepth = curContact.depth; - } - } - } if (ncontacts > 0) { - maxDepthContact.CharacterFeet = FeetCollision; + ContactPoint maxDepthContact = new ContactPoint( + new Vector3(maxContact.pos.X, maxContact.pos.Y, maxContact.pos.Z), + new Vector3(minContact.normal.X, minContact.normal.Y, minContact.normal.Z), + maxContact.depth, FeetCollision + ); collision_accounting_events(p1, p2, maxDepthContact); } -/* - if (notskipedcount > geomContactPointsStartthrottle) - { - // If there are more then 3 contact points, it's likely - // that we've got a pile of objects, so ... - // We don't want to send out hundreds of terse updates over and over again - // so lets throttle them and send them again after it's somewhat sorted out. - this needs checking so out for now - if (b1 != IntPtr.Zero) - p1.ThrottleUpdates = true; - if (b2 != IntPtr.Zero) - p2.ThrottleUpdates = true; - - } - */ } private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) @@ -1286,10 +1152,7 @@ namespace OpenSim.Region.Physics.OdePlugin // chr.CollidingGround = false; not done here chr.CollidingObj = false; // do colisions with static space - d.SpaceCollide2(StaticSpace, chr.topbox, IntPtr.Zero, nearCallback); - d.SpaceCollide2(StaticSpace, chr.midbox, IntPtr.Zero, nearCallback); - d.SpaceCollide2(StaticSpace, chr.feetbox, IntPtr.Zero, nearCallback); - d.SpaceCollide2(StaticSpace, chr.bonebox, IntPtr.Zero, nearCallback); + d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback); // chars with chars d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); @@ -1346,7 +1209,7 @@ namespace OpenSim.Region.Physics.OdePlugin // and with chars try { - d.SpaceCollide2(ActiveSpace, CharsSpace,IntPtr.Zero, nearCallback); + d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback); } catch (AccessViolationException) { @@ -1837,7 +1700,7 @@ namespace OpenSim.Region.Physics.OdePlugin foreach (OdeCharacter actor in _characters) { if (actor != null) - actor.Move(ODE_STEPSIZE, defects); + actor.Move(defects); } if (defects.Count != 0) {