diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
index 144d473c25..ac007aef2e 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
@@ -28,7 +28,7 @@
using log4net;
using System;
using System.Collections.Generic;
-using System.Reflection;
+using System.Runtime.InteropServices;
using OpenSim.Framework;
using OpenMetaverse;
@@ -62,6 +62,14 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
public bool MouseLook;
}
+ public struct AABB2D
+ {
+ public float minx;
+ public float miny;
+ public float maxx;
+ public float maxy;
+ }
+
public struct ContactPoint
{
public Vector3 Position;
@@ -158,7 +166,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
public abstract class PhysicsActor
{
-// private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+ //private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
public delegate void RequestTerseUpdate();
public delegate void CollisionUpdate(EventArgs e);
@@ -201,6 +209,8 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
cdata.bounce = 0;
}
+ public AABB2D _AABB2D;
+
public abstract bool Stopped { get; }
public abstract Vector3 Size { get; set; }
@@ -258,38 +268,17 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
public virtual void RequestPhysicsterseUpdate()
{
- // Make a temporary copy of the event to avoid possibility of
- // a race condition if the last subscriber unsubscribes
- // immediately after the null check and before the event is raised.
- RequestTerseUpdate handler = OnRequestTerseUpdate;
-
- if (handler != null)
- {
- handler();
- }
+ OnRequestTerseUpdate?.Invoke();
}
public virtual void RaiseOutOfBounds(Vector3 pos)
{
- // Make a temporary copy of the event to avoid possibility of
- // a race condition if the last subscriber unsubscribes
- // immediately after the null check and before the event is raised.
- OutOfBounds handler = OnOutOfBounds;
-
- if (handler != null)
- {
- handler(pos);
- }
+ OnOutOfBounds?.Invoke(pos);
}
public virtual void SendCollisionUpdate(EventArgs e)
{
- CollisionUpdate handler = OnCollisionUpdate;
-
-// m_log.DebugFormat("[PHYSICS ACTOR]: Sending collision for {0}", LocalID);
-
- if (handler != null)
- handler(e);
+ OnCollisionUpdate?.Invoke(e);
}
public virtual void SetMaterial (int material) { }
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs
index ca2be8edbc..7ee6a976f5 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs
@@ -88,13 +88,11 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
public struct ContactResult
{
public Vector3 Pos;
+ public Vector3 Normal;
public float Depth;
public uint ConsumerID;
- public Vector3 Normal;
}
-
-
public abstract class PhysicsScene
{
// private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs
index e6eebab663..66f8ef5845 100644
--- a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs
@@ -31,6 +31,7 @@
using System;
using System.Collections.Generic;
using System.Reflection;
+using System.Runtime.CompilerServices;
using OpenMetaverse;
using OpenSim.Framework;
using OpenSim.Region.PhysicsModules.SharedBase;
@@ -71,7 +72,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
- private Vector3 _position;
+ internal Vector3 _position;
private Vector3 _zeroPosition;
private Vector3 _velocity;
private Vector3 _acceleration;
@@ -87,6 +88,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public readonly float m_density = 60f;
private bool m_pidControllerActive = true;
+ public float CapsuleRadius;
+ public float CapsuleSizeZ;
+
+ public int _charsListIndex;
+
public int m_bodydisablecontrol = 0;
const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80))
@@ -194,6 +200,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
m_size.Y = pSize.Y > 0.01f ? pSize.Y : 0.01f;
m_size.Z = pSize.Z > 0.01f ? pSize.Z : 0.01f;
+ CapsuleRadius = m_size.X;
+ if (CapsuleRadius < m_size.Y)
+ CapsuleRadius = m_size.Y;
+ CapsuleRadius *= 0.5f;
+ CapsuleSizeZ = 0.5f * pSize.Z;
+
m_feetOffset = pfeetOffset;
m_orientation = Quaternion.Identity;
m_orientation2D = Quaternion.Identity;
@@ -225,21 +237,26 @@ namespace OpenSim.Region.PhysicsModule.ubOde
Name = avName;
+ UpdateShortAABB();
+
AddChange(changes.Add, null);
}
public override int PhysicsActorType
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return (int)ActorTypes.Agent;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
return;
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void getContactData(ref ContactData cdata)
{
cdata.mu = m_frictionmu;
@@ -258,10 +275,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private bool m_alwaysRun = false;
public override bool SetAlwaysRun
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_alwaysRun;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
m_alwaysRun = value;
@@ -271,6 +290,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private readonly uint m_localID = 0;
public override uint LocalID
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_localID;
@@ -279,6 +299,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override PhysicsActor ParentActor
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return (PhysicsActor)this;
@@ -287,6 +308,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override bool Grabbed
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
return;
@@ -295,6 +317,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override bool Selected
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
return;
@@ -304,10 +327,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private float m_buoyancy = 0f;
public override float Buoyancy
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_buoyancy;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
m_buoyancy = value;
@@ -316,6 +341,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override bool FloatOnWater
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
return;
@@ -324,10 +350,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override bool IsPhysical
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_isPhysical;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
return;
@@ -336,10 +364,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override bool ThrottleUpdates
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return false;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
return;
@@ -348,10 +378,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override bool Flying
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_flying;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
m_flying = value;
@@ -365,10 +397,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
public override bool IsColliding
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return (m_iscolliding || m_iscollidingGround);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
if (value)
@@ -400,10 +434,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
public override bool CollidingGround
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_iscollidingGround;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
/* we now control this
@@ -434,10 +470,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
public override bool CollidingObj
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_iscollidingObj;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
if (value)
@@ -472,6 +510,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
/// The PID Controller will turn on all by itself in many situations
///
///
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void SetPidStatus(bool status)
{
m_pidControllerActive = status;
@@ -479,6 +518,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override bool Stopped
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return _zeroFlag;
@@ -492,10 +532,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
public override Vector3 Position
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return _position;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
if (value.IsFinite())
@@ -514,10 +556,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override Vector3 RotationalVelocity
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_rotationalVelocity;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
m_rotationalVelocity = value;
@@ -530,10 +574,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
public override Vector3 Size
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_size;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
if (value.IsFinite())
@@ -554,6 +600,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void setAvatarSize(Vector3 size, float feetOffset)
{
if (size.IsFinite())
@@ -583,6 +630,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
public override float Mass
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_mass;
@@ -605,6 +653,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override Vector3 Force
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_targetVelocity;
@@ -685,10 +734,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override Vector3 Velocity
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return _velocity;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
if (value.IsFinite())
@@ -704,10 +755,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override Vector3 TargetVelocity
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_targetVelocity;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
if (value.IsFinite())
@@ -723,10 +776,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override Vector3 Torque
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return Vector3.Zero;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
return;
@@ -735,6 +790,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override float CollisionScore
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return 0f;
@@ -746,6 +802,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override bool Kinematic
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return false;
@@ -757,10 +814,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override Quaternion Orientation
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_orientation;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
//fakeori = value;
@@ -772,6 +831,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override Vector3 Acceleration
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return _acceleration;
@@ -781,6 +841,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void SetAcceleration(Vector3 accel)
{
m_pidControllerActive = true;
@@ -794,6 +855,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
/// The PID controller takes this target velocity and tries to make it a reality
///
///
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
@@ -814,6 +876,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
//m_lastUpdateSent = false;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void AvatarJump(float impulseZ)
{
// convert back to force and remove mass effect
@@ -824,6 +887,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void SetMomentum(Vector3 momentum)
{
if (momentum.IsFinite())
@@ -831,7 +895,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
- private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
+ private void AvatarGeomAndBodyCreate()
{
float sx = m_size.X;
float sy = m_size.Y;
@@ -850,22 +914,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
AvaAvaSizeYsq = 0.5f * sy;
AvaAvaSizeYsq *= AvaAvaSizeYsq;
- //m_parent_scene.waitForSpaceUnlock(m_parent_scene.CharsSpace);
+ CapsuleRadius = sx;
+ if (sy > CapsuleRadius)
+ CapsuleRadius = sy;
+ float l = sz - CapsuleRadius;
+ CapsuleRadius *= 0.5f;
+ CapsuleSizeZ = 0.5f * sz;
- //collider = SafeNativeMethods.SimpleSpaceCreate(m_parent_scene.CharsSpace);
- //SafeNativeMethods.SpaceSetSublevel(collider, 0);
- //SafeNativeMethods.SpaceSetCleanup(collider, false);
- //SafeNativeMethods.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
- //SafeNativeMethods.GeomSetCollideBits(collider, (uint)m_collisionFlags);
-
- float r = sx;
- if (sy > r)
- r = sy;
- float l = sz - r;
- r *= 0.5f;
-
- //capsule = SafeNativeMethods.CreateCapsule(collider, r, l);
- collider = SafeNativeMethods.CreateCapsule(m_parent_scene.CharsSpace, r, l);
+ collider = SafeNativeMethods.CreateCapsule(m_parent_scene.CharsSpace, CapsuleRadius, l);
SafeNativeMethods.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
SafeNativeMethods.GeomSetCollideBits(collider, (uint)m_collisionFlags);
@@ -889,11 +945,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
SafeNativeMethods.BodySetAutoDisableFlag(Body, true);
m_bodydisablecontrol = 0;
- SafeNativeMethods.BodySetPosition(Body, npositionX, npositionY, npositionZ);
-
- _position.X = npositionX;
- _position.Y = npositionY;
- _position.Z = npositionZ;
+ SafeNativeMethods.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
SafeNativeMethods.BodySetMass(Body, ref ShellMass);
//SafeNativeMethods.GeomSetBody(capsule, Body);
@@ -941,6 +993,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
/// Destroys the avatar body and geom
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void AvatarGeomAndBodyDestroy()
{
// Kill the Amotor
@@ -1015,6 +1068,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
y = tx * sin + y * cos;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
internal bool Collide(IntPtr me, IntPtr other, bool reverse, ref SafeNativeMethods.ContactGeomClass contact,
ref SafeNativeMethods.ContactGeomClass altContact, ref bool useAltcontact, ref bool feetcollision)
{
@@ -1041,7 +1095,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (r > 1.0f)
return false;
- float dp = 1.0f - (float)Math.Sqrt((double)r);
+ float dp = 1.0f - (float)Math.Sqrt(r);
if (dp > 0.05f)
dp = 0.05f;
@@ -1589,6 +1643,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// update our local ideia of position velocity and aceleration
_position = localpos;
+ UpdateShortAABB();
if (_zeroFlag)
{
@@ -1611,6 +1666,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void round(ref Vector3 v, int digits)
{
v.X = (float)Math.Round(v.X, digits);
@@ -1618,6 +1674,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
v.Z = (float)Math.Round(v.Z, digits);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void SetSmooth(ref Vector3 dst, ref Vector3 value)
{
dst.X = 0.1f * dst.X + 0.9f * value.X;
@@ -1625,6 +1682,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
{
dst.X = 0.4f * dst.X + 0.6f * value.X;
@@ -1652,11 +1710,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
/// Cleanup the things we use in the scene.
///
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Destroy()
{
AddChange(changes.Remove, null);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void CrossingFailure()
{
}
@@ -1670,6 +1730,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
public override bool PIDActive
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_pidControllerActive;
@@ -1687,6 +1748,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override float PIDHoverHeight
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
AddChange(changes.PIDHoverHeight, value);
@@ -1694,10 +1756,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
public override bool PIDHoverActive
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_useHoverPID;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
AddChange(changes.PIDHoverActive, value);
@@ -1706,6 +1770,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override PIDHoverType PIDHoverType
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
AddChange(changes.PIDHoverType, value);
@@ -1714,6 +1779,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public override float PIDHoverTau
{
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
set
{
float tmp = 0;
@@ -1755,6 +1821,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void SubscribeEvents(int ms)
{
m_eventsubscription = ms;
@@ -1763,6 +1830,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
SentEmptyCollisionsEvent = false;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void UnSubscribeEvents()
{
m_eventsubscription = 0;
@@ -1771,6 +1839,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
CollisionEventsThisFrame.Clear();
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
{
lock (CollisionEventsThisFrame)
@@ -1812,6 +1881,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override bool SubscribedEvents()
{
if (m_eventsubscription > 0)
@@ -1819,6 +1889,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
return false;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changePhysicsStatus(bool NewStatus)
{
if (NewStatus != m_isPhysical)
@@ -1827,16 +1898,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
AvatarGeomAndBodyDestroy();
- AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
+ AvatarGeomAndBodyCreate();
- //m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this;
- m_parent_scene.actor_name_map[collider] = (PhysicsActor)this;
+ m_parent_scene.actor_name_map[collider] = this;
m_parent_scene.AddCharacter(this);
}
else
{
m_parent_scene.RemoveCollisionEventReporting(this);
m_parent_scene.RemoveCharacter(this);
+
// destroy avatar capsule and related ODE data
AvatarGeomAndBodyDestroy();
}
@@ -1845,20 +1916,24 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeAdd()
{
changePhysicsStatus(true);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeRemove()
{
changePhysicsStatus(false);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeShape(PrimitiveBaseShape arg)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeAvatarSize(strAvatarSize st)
{
m_feetOffset = st.offset;
@@ -1893,14 +1968,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
AvaAvaSizeYsq = 0.5f * sy;
AvaAvaSizeYsq *= AvaAvaSizeYsq;
- float r = sx;
- if (sy > r)
- r = sy;
- float l = sz - r;
- r *= 0.5f;
-
+ CapsuleRadius = sx;
+ if (sy > CapsuleRadius)
+ CapsuleRadius = sy;
+ float l = sz - CapsuleRadius;
+ CapsuleRadius *= 0.5f;
+ CapsuleSizeZ= 0.5f * sz;
//SafeNativeMethods.GeomCapsuleSetParams(capsule, r, l);
- SafeNativeMethods.GeomCapsuleSetParams(collider, r, l);
+ SafeNativeMethods.GeomCapsuleSetParams(collider, CapsuleRadius, l);
m_mass = m_density * sx * sy * sz; // update mass
m_massInvTimeScaled = m_mass * m_sceneInverseTimeStep;
@@ -1914,6 +1989,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
_position.Z += (sz - oldsz) * 0.5f;
SafeNativeMethods.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+ UpdateShortAABB();
+
m_bodydisablecontrol = 0;
_zeroFlag = false;
_velocity = Vector3.Zero;
@@ -1928,6 +2005,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changePosition(Vector3 newPos)
{
if (Body != IntPtr.Zero)
@@ -1935,13 +2013,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
SafeNativeMethods.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
SafeNativeMethods.BodyEnable(Body);
}
-
+ UpdateShortAABB();
_position = newPos;
m_freemove = false;
_zeroFlag = false;
m_pidControllerActive = true;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeOrientation(Quaternion newOri)
{
if (m_orientation.NotEqual(newOri))
@@ -1980,6 +2059,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeVelocity(Vector3 newVel)
{
_velocity = newVel;
@@ -1992,6 +2072,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeTargetVelocity(Vector3 newVel)
{
//m_pidControllerActive = true;
@@ -2001,42 +2082,52 @@ namespace OpenSim.Region.PhysicsModule.ubOde
SafeNativeMethods.BodyEnable(Body);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeSetTorque(Vector3 newTorque)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeAddForce(Vector3 newForce)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeAddAngularForce(Vector3 arg)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeAngularLock(byte arg)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeFloatOnWater(bool arg)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeVolumedetetion(bool arg)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeSelectedStatus(bool arg)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeDisable(bool arg)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeBuilding(bool arg)
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void setFreeMove()
{
m_pidControllerActive = true;
@@ -2054,6 +2145,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
CollisionEventsThisFrame.Clear();
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeForce(Vector3 newForce)
{
setFreeMove();
@@ -2067,6 +2159,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
// for now momentum is actually velocity
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeMomentum(Vector3 newmomentum)
{
_velocity = newmomentum;
@@ -2079,6 +2172,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changePIDHoverHeight(float val)
{
m_PIDHoverHeight = val;
@@ -2086,16 +2180,19 @@ namespace OpenSim.Region.PhysicsModule.ubOde
m_useHoverPID = false;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changePIDHoverType(PIDHoverType type)
{
m_PIDHoverType = type;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changePIDHoverTau(float tau)
{
m_PIDHoverTau = tau;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changePIDHoverActive(bool active)
{
m_useHoverPID = active;
@@ -2105,6 +2202,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public void UpdateShortAABB()
+ {
+ _AABB2D.minx = _position.X - CapsuleRadius;
+ _AABB2D.maxx = _position.X + CapsuleRadius;
+ _AABB2D.miny = _position.Y - CapsuleRadius;
+ _AABB2D.maxy = _position.Y + CapsuleRadius;
+ }
+
public bool DoAChange(changes what, object arg)
{
if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
@@ -2251,6 +2357,5 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public Vector3 size;
public float offset;
}
-
}
}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
index a65803e2fc..341e335ce3 100644
--- a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
@@ -33,6 +33,7 @@ using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
+using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Threading;
using log4net;
@@ -171,10 +172,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
//private int threadid = 0;
- const SafeNativeMethods.ContactFlags comumContactFlags = SafeNativeMethods.ContactFlags.Bounce | SafeNativeMethods.ContactFlags.Approx1;
- const float comumContactERP = 0.75f;
- const float comumContactCFM = 0.0001f;
- const float comumContactSLIP = 0f;
+ const SafeNativeMethods.ContactFlags commomContactFlags = SafeNativeMethods.ContactFlags.Bounce | SafeNativeMethods.ContactFlags.Approx1;
+ const float commomContactERP = 0.75f;
+ const float commonContactCFM = 0.0001f;
+ const float commomContactSLIP = 0f;
float TerrainBounce = 0.001f;
float TerrainFriction = 0.3f;
@@ -220,12 +221,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public int bodyFramesAutoDisable = 10;
- private SafeNativeMethods.NearCallback nearCallback;
-
private readonly Dictionary _prims = new Dictionary();
private readonly HashSet _characters = new HashSet();
private readonly HashSet _activeprims = new HashSet();
private readonly HashSet _activegroups = new HashSet();
+ public List _charactersList;
public readonly ConcurrentQueue ChangesQueue = new ConcurrentQueue();
@@ -235,7 +235,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private readonly List _collisionEventPrim = new List();
private readonly List _collisionEventPrimRemove = new List();
- private readonly HashSet _badCharacter = new HashSet();
+ private readonly List _badCharacter = new List();
public readonly Dictionary actor_name_map = new Dictionary();
private float contactsurfacelayer = 0.002f;
@@ -243,12 +243,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private int contactsPerCollision = 80;
internal IntPtr ContactgeomsArray = IntPtr.Zero;
private IntPtr GlobalContactsArray = IntPtr.Zero;
- private SafeNativeMethods.Contact SharedTmpcontact = new SafeNativeMethods.Contact();
+ private SafeNativeMethods.Contact contactSharedForJoints = new SafeNativeMethods.Contact();
- const int maxContactsbeforedeath = 6000;
- private volatile int m_global_contactcount = 0;
+ const int maxContactJoints = 6000;
+ private volatile int ContactJointCount = 0;
- private IntPtr contactgroup;
+ private IntPtr JointContactGroup;
public readonly ContactData[] m_materialContactsData = new ContactData[8];
@@ -279,7 +279,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public readonly object OdeLock = new object();
public static readonly object SimulationLock = new object();
-
public IMesher mesher;
public IConfigSource m_config;
@@ -339,7 +338,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
SafeNativeMethods.AllocateODEDataForThread(~0U);
- nearCallback = near;
+ _charactersList = new List(m_frameWorkScene.RegionInfo.AgentCapacity);
m_rayCastManager = new ODERayCastRequestManager(this);
@@ -408,7 +407,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
));
SafeNativeMethods.GeomSetCollideBits(StaticSpace, 0);
- contactgroup = SafeNativeMethods.JointGroupCreate(maxContactsbeforedeath + 1);
+ JointContactGroup = SafeNativeMethods.JointGroupCreate(maxContactJoints + 1);
//contactgroup
SafeNativeMethods.WorldSetAutoDisableFlag(world, false);
@@ -457,8 +456,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI;
maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity;
- SafeNativeMethods.WorldSetCFM(world, comumContactCFM);
- SafeNativeMethods.WorldSetERP(world, comumContactERP);
+ SafeNativeMethods.WorldSetCFM(world, commonContactCFM);
+ SafeNativeMethods.WorldSetERP(world, commomContactERP);
SafeNativeMethods.WorldSetGravity(world, gravityx, gravityy, gravityz);
@@ -477,22 +476,22 @@ namespace OpenSim.Region.PhysicsModule.ubOde
odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f);
ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * SafeNativeMethods.ContactGeomClass.unmanagedSizeOf);
- GlobalContactsArray = Marshal.AllocHGlobal((maxContactsbeforedeath + 100) * SafeNativeMethods.Contact.unmanagedSizeOf);
+ GlobalContactsArray = Marshal.AllocHGlobal((maxContactJoints + 100) * SafeNativeMethods.Contact.unmanagedSizeOf);
- SharedTmpcontact.geom.g1 = IntPtr.Zero;
- SharedTmpcontact.geom.g2 = IntPtr.Zero;
+ contactSharedForJoints.geom.g1 = IntPtr.Zero;
+ contactSharedForJoints.geom.g2 = IntPtr.Zero;
- SharedTmpcontact.geom.side1 = -1;
- SharedTmpcontact.geom.side2 = -1;
+ contactSharedForJoints.geom.side1 = -1;
+ contactSharedForJoints.geom.side2 = -1;
- SharedTmpcontact.surface.mode = comumContactFlags;
- SharedTmpcontact.surface.mu = 0;
- SharedTmpcontact.surface.bounce = 0;
- SharedTmpcontact.surface.bounce_vel = 1.5f;
- SharedTmpcontact.surface.soft_cfm = comumContactCFM;
- SharedTmpcontact.surface.soft_erp = comumContactERP;
- SharedTmpcontact.surface.slip1 = comumContactSLIP;
- SharedTmpcontact.surface.slip2 = comumContactSLIP;
+ contactSharedForJoints.surface.mode = commomContactFlags;
+ contactSharedForJoints.surface.mu = 0;
+ contactSharedForJoints.surface.bounce = 0;
+ contactSharedForJoints.surface.bounce_vel = 1.5f;
+ contactSharedForJoints.surface.soft_cfm = commonContactCFM;
+ contactSharedForJoints.surface.soft_erp = commomContactERP;
+ contactSharedForJoints.surface.slip1 = commomContactSLIP;
+ contactSharedForJoints.surface.slip2 = commomContactSLIP;
m_materialContactsData[(int)Material.Stone].mu = 0.8f;
m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
@@ -539,24 +538,23 @@ namespace OpenSim.Region.PhysicsModule.ubOde
#region Collision Detection
// sets a global contact for a joint for contactgeom , and base contact description)
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private IntPtr CreateContacJoint(ref SafeNativeMethods.ContactGeomClass contactGeom, bool smooth)
{
- if (m_global_contactcount >= maxContactsbeforedeath)
+ if (ContactJointCount >= maxContactJoints)
return IntPtr.Zero;
- m_global_contactcount++;
- if(smooth)
- SharedTmpcontact.geom.depth = contactGeom.depth * 0.05f;
- else
- SharedTmpcontact.geom.depth = contactGeom.depth;
- SharedTmpcontact.geom.pos = contactGeom.pos;
- SharedTmpcontact.geom.normal = contactGeom.normal;
+ ContactJointCount++;
+ contactSharedForJoints.geom.depth = smooth ? contactGeom.depth * 0.05f : contactGeom.depth;
+ contactSharedForJoints.geom.pos = contactGeom.pos;
+ contactSharedForJoints.geom.normal = contactGeom.normal;
- IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * SafeNativeMethods.Contact.unmanagedSizeOf));
- Marshal.StructureToPtr(SharedTmpcontact, contact, false);
- return SafeNativeMethods.JointCreateContactPtr(world, contactgroup, contact);
+ IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(ContactJointCount * SafeNativeMethods.Contact.unmanagedSizeOf));
+ Marshal.StructureToPtr(contactSharedForJoints, contact, false);
+ return SafeNativeMethods.JointCreateContactPtr(world, JointContactGroup, contact);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private bool GetCurContactGeom(int index, ref SafeNativeMethods.ContactGeomClass newcontactgeom)
{
if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
@@ -567,6 +565,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
return true;
}
+ SafeNativeMethods.ContactGeomClass CurWorkContact = new SafeNativeMethods.ContactGeomClass();
+ SafeNativeMethods.ContactGeomClass altWorkContact = new SafeNativeMethods.ContactGeomClass();
+
///
/// This is our near callback. A geometry is near a body
///
@@ -574,11 +575,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
/// a geometry or space
/// another geometry or space
///
-
- private void near(IntPtr space, IntPtr g1, IntPtr g2)
+ private void DefaultNearCallback(IntPtr space, IntPtr g1, IntPtr g2)
{
// no lock here! It's invoked from within Simulate(), which is thread-locked
- if (m_global_contactcount >= maxContactsbeforedeath)
+ if (ContactJointCount >= maxContactJoints)
return;
// Test if we're colliding a geom with a space.
@@ -593,7 +593,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// contact points in the space
try
{
- SafeNativeMethods.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+ SafeNativeMethods.SpaceCollide2(g1, g2, IntPtr.Zero, DefaultNearCallback);
}
catch (AccessViolationException)
{
@@ -612,7 +612,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (SafeNativeMethods.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
SafeNativeMethods.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
{
- int cflags = unchecked ((int)(1 | SafeNativeMethods.CONTACTS_UNIMPORTANT));
+ int cflags = unchecked((int)(1 | SafeNativeMethods.CONTACTS_UNIMPORTANT));
count = SafeNativeMethods.CollidePtr(g1, g2, cflags, ContactgeomsArray, SafeNativeMethods.ContactGeomClass.unmanagedSizeOf);
}
else
@@ -634,7 +634,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (count == 0)
return;
-
// try get physical actors
if (!actor_name_map.TryGetValue(g1, out PhysicsActor p1))
{
@@ -649,17 +648,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
// get first contact
- SafeNativeMethods.ContactGeomClass curContact = new SafeNativeMethods.ContactGeomClass();
- if (!GetCurContactGeom(0, ref curContact))
+ if (!GetCurContactGeom(0, ref CurWorkContact))
return;
// do volume detection case
if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
{
ContactPoint volDepthContact = 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
+ new Vector3(CurWorkContact.pos.X, CurWorkContact.pos.Y, CurWorkContact.pos.Z),
+ new Vector3(CurWorkContact.normal.X, CurWorkContact.normal.Y, CurWorkContact.normal.Z),
+ CurWorkContact.depth, false
);
collision_accounting_events(p1, p2, volDepthContact);
@@ -706,9 +704,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
break;
case (int)ActorTypes.Prim:
-// Vector3 relV = p1.rootVelocity - p2.rootVelocity;
-// float relVlenSQ = relV.LengthSquared();
-// if (relVlenSQ > 0.0001f)
+ //Vector3 relV = p1.rootVelocity - p2.rootVelocity;
+ //float relVlenSQ = relV.LengthSquared();
+ //if (relVlenSQ > 0.0001f)
{
p1.CollidingObj = true;
p2.CollidingObj = true;
@@ -721,7 +719,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
//if (relVlenSQ > 0.01f)
// mu *= frictionMovementMult;
- if(SafeNativeMethods.GeomGetClass(g2) == SafeNativeMethods.GeomClassID.TriMeshClass &&
+ if (SafeNativeMethods.GeomGetClass(g2) == SafeNativeMethods.GeomClassID.TriMeshClass &&
SafeNativeMethods.GeomGetClass(g1) == SafeNativeMethods.GeomClassID.TriMeshClass)
smoothMesh = true;
break;
@@ -736,7 +734,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// mu *= frictionMovementMult;
p1.CollidingGround = true;
- if(SafeNativeMethods.GeomGetClass(g1) == SafeNativeMethods.GeomClassID.TriMeshClass)
+ if (SafeNativeMethods.GeomGetClass(g1) == SafeNativeMethods.GeomClassID.TriMeshClass)
smoothMesh = true;
break;
@@ -762,7 +760,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
//if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
// mu *= frictionMovementMult;
- if(SafeNativeMethods.GeomGetClass(g2) == SafeNativeMethods.GeomClassID.TriMeshClass)
+ if (SafeNativeMethods.GeomGetClass(g2) == SafeNativeMethods.GeomClassID.TriMeshClass)
smoothMesh = true;
}
else
@@ -784,25 +782,22 @@ namespace OpenSim.Region.PhysicsModule.ubOde
int i = 0;
ContactPoint maxDepthContact = new ContactPoint();
- ContactPoint minDepthContact = new ContactPoint();
float minDepth = float.MaxValue;
float maxDepth = float.MinValue;
- SharedTmpcontact.geom.depth = 0;
- SharedTmpcontact.surface.mu = mu;
- SharedTmpcontact.surface.bounce = bounce;
+ contactSharedForJoints.surface.mu = mu;
+ contactSharedForJoints.surface.bounce = bounce;
- SafeNativeMethods.ContactGeomClass altContact = new SafeNativeMethods.ContactGeomClass();
bool useAltcontact;
bool noskip;
- if(dop1ava || dop2ava)
+ if (dop1ava || dop2ava)
smoothMesh = false;
IntPtr b1 = SafeNativeMethods.GeomGetBody(g1);
IntPtr b2 = SafeNativeMethods.GeomGetBody(g2);
-
+
while (true)
{
noskip = true;
@@ -810,7 +805,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (dop1ava)
{
- if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+ if ((((OdeCharacter)p1).Collide(g1, g2, false, ref CurWorkContact, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
{
if (p2.PhysicsActorType == (int)ActorTypes.Agent)
{
@@ -825,26 +820,25 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
else if (dop2ava)
{
- if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+ if ((((OdeCharacter)p2).Collide(g2, g1, true, ref CurWorkContact, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
{
- if (p1.PhysicsActorType == (int)ActorTypes.Agent)
+ if (p1.PhysicsActorType == (int)ActorTypes.Agent)
{
p1.CollidingObj = true;
p2.CollidingObj = true;
}
- else if (p1.rootVelocity.LengthSquared() > 0.0f)
- p1.CollidingObj = true;
+ else if (p1.rootVelocity.LengthSquared() > 0.0f)
+ p1.CollidingObj = true;
}
- else
- noskip = false;
+ else
+ noskip = false;
}
if (noskip)
{
- if(useAltcontact)
- Joint = CreateContacJoint(ref altContact,smoothMesh);
- else
- Joint = CreateContacJoint(ref curContact,smoothMesh);
+ Joint = useAltcontact ?
+ CreateContacJoint(ref altWorkContact, smoothMesh) :
+ CreateContacJoint(ref CurWorkContact, smoothMesh);
if (Joint == IntPtr.Zero)
break;
@@ -852,39 +846,137 @@ namespace OpenSim.Region.PhysicsModule.ubOde
ncontacts++;
- if (curContact.depth > maxDepth)
+ if (CurWorkContact.depth > maxDepth)
{
- maxDepth = curContact.depth;
+ maxDepth = CurWorkContact.depth;
maxDepthContact.PenetrationDepth = maxDepth;
- maxDepthContact.Position.X = curContact.pos.X;
- maxDepthContact.Position.Y = curContact.pos.Y;
- maxDepthContact.Position.Z = curContact.pos.Z;
+ maxDepthContact.Position.X = CurWorkContact.pos.X;
+ maxDepthContact.Position.Y = CurWorkContact.pos.Y;
+ maxDepthContact.Position.Z = CurWorkContact.pos.Z;
maxDepthContact.CharacterFeet = FeetCollision;
}
- if (curContact.depth < minDepth)
+ if (CurWorkContact.depth < minDepth)
{
- minDepth = curContact.depth;
- minDepthContact.PenetrationDepth = minDepth;
- minDepthContact.SurfaceNormal.X = curContact.normal.X;
- minDepthContact.SurfaceNormal.Y = curContact.normal.Y;
- minDepthContact.SurfaceNormal.Z = curContact.normal.Z;
+ minDepth = CurWorkContact.depth;
+ maxDepthContact.SurfaceNormal.X = CurWorkContact.normal.X;
+ maxDepthContact.SurfaceNormal.Y = CurWorkContact.normal.Y;
+ maxDepthContact.SurfaceNormal.Z = CurWorkContact.normal.Z;
}
}
if (++i >= count)
break;
- if (!GetCurContactGeom(i, ref curContact))
+ if (!GetCurContactGeom(i, ref CurWorkContact))
break;
}
if (ncontacts > 0)
{
- maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
- maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
- maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
+ collision_accounting_events(p1, p2, maxDepthContact);
+ }
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ private void CollideCharChar(OdeCharacter p1, OdeCharacter p2)
+ {
+ if (ContactJointCount >= maxContactJoints)
+ return;
+ // Figure out how many contact points we have
+ int count = 0;
+ try
+ {
+ count = SafeNativeMethods.CollidePtr(p1.collider, p2.collider, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.ContactGeomClass.unmanagedSizeOf);
+ }
+ 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);
+ base.TriggerPhysicsBasedRestart();
+ }
+ catch (Exception e)
+ {
+ m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+ return;
+ }
+
+ // contacts done
+ if (count == 0)
+ return;
+
+ // get first contact
+ if (!GetCurContactGeom(0, ref CurWorkContact))
+ return;
+
+ ContactData contactdata1 = new ContactData(0, 0, false);
+ ContactData contactdata2 = new ContactData(0, 0, false);
+
+ IntPtr Joint;
+ bool FeetCollision = false;
+ int ncontacts = 0;
+
+ int i = 0;
+
+ ContactPoint maxDepthContact = new ContactPoint();
+
+ float minDepth = float.MaxValue;
+ float maxDepth = float.MinValue;
+
+ contactSharedForJoints.surface.mu = 0;
+ contactSharedForJoints.surface.bounce = 0;
+
+ bool useAltcontact;
+
+ while (true)
+ {
+ useAltcontact = false;
+
+ if (p1.Collide(p1.collider, p2.collider, false, ref CurWorkContact, ref altWorkContact, ref useAltcontact, ref FeetCollision))
+ {
+ p1.CollidingObj = true;
+ p1.CollidingObj = true;
+
+ if (useAltcontact)
+ Joint = CreateContacJoint(ref altWorkContact, false);
+ else
+ Joint = CreateContacJoint(ref CurWorkContact, false);
+ if (Joint == IntPtr.Zero)
+ break;
+
+ SafeNativeMethods.JointAttach(Joint, p1.Body, p2.Body);
+
+ ncontacts++;
+
+ if (CurWorkContact.depth > maxDepth)
+ {
+ maxDepth = CurWorkContact.depth;
+ maxDepthContact.PenetrationDepth = maxDepth;
+ maxDepthContact.Position.X = CurWorkContact.pos.X;
+ maxDepthContact.Position.Y = CurWorkContact.pos.Y;
+ maxDepthContact.Position.Z = CurWorkContact.pos.Z;
+ maxDepthContact.CharacterFeet = FeetCollision;
+ }
+
+ if (CurWorkContact.depth < minDepth)
+ {
+ minDepth = CurWorkContact.depth;
+ maxDepthContact.SurfaceNormal.X = CurWorkContact.normal.X;
+ maxDepthContact.SurfaceNormal.Y = CurWorkContact.normal.Y;
+ maxDepthContact.SurfaceNormal.Z = CurWorkContact.normal.Z;
+ }
+ }
+
+ if (++i >= count)
+ break;
+
+ if (!GetCurContactGeom(i, ref CurWorkContact))
+ break;
+ }
+
+ if (ncontacts > 0)
+ {
collision_accounting_events(p1, p2, maxDepthContact);
}
}
@@ -911,10 +1003,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
return;
Vector3 vel = Vector3.Zero;
- if (p2 != null && p2.IsPhysical)
+ if (p2.IsPhysical)
vel = p2.rootVelocity;
- if (p1 != null && p1.IsPhysical)
+ if (p1.IsPhysical)
vel -= p1.rootVelocity;
contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
@@ -982,32 +1074,80 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private void collision_optimized()
{
lock (_characters)
- {
+ {
try
{
- foreach (OdeCharacter chr in _characters)
+ if (_charactersList.Count == 1)
{
- if (chr == null)
- continue;
-
+ OdeCharacter chr = _charactersList[0];
if (chr.Colliderfilter < -1)
chr.Colliderfilter = -1;
else
{
chr.IsColliding = false;
- // chr.CollidingGround = false; not done here
chr.CollidingObj = false;
- if(chr.Body == IntPtr.Zero || chr.collider == IntPtr.Zero )
- continue;
-
- // do colisions with static space
- SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
- SafeNativeMethods.SpaceCollide2(chr.collider, CharsSpace, IntPtr.Zero, nearCallback);
- SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, nearCallback);
+ if (chr.Body != IntPtr.Zero && chr.collider != IntPtr.Zero)
+ {
+ SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, DefaultNearCallback);
+ SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, DefaultNearCallback);
+ }
}
- // no coll with gnd
}
+ else if (_charactersList.Count > 1)
+ {
+ for (int i = 0; i < _charactersList.Count; ++i)
+ {
+ OdeCharacter chr = _charactersList[i];
+
+ if (chr.Colliderfilter < -1)
+ chr.Colliderfilter = -1;
+ else
+ {
+ chr.IsColliding = false;
+ chr.CollidingObj = false;
+
+ // do colisions with static space
+ SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, DefaultNearCallback);
+ SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, DefaultNearCallback);
+
+ float mx = chr._AABB2D.minx;
+ float Mx = chr._AABB2D.maxx;
+ float my = chr._AABB2D.miny;
+ float My = chr._AABB2D.maxy;
+
+ for (int j = i + 1; j < _charactersList.Count; ++j)
+ {
+ OdeCharacter chr2 = _charactersList[j];
+ if (chr2.Colliderfilter < -1)
+ continue;
+
+ if(Mx < chr2._AABB2D.minx ||
+ mx > chr2._AABB2D.maxx ||
+ My < chr2._AABB2D.miny ||
+ my > chr2._AABB2D.maxy)
+ continue;
+ /*
+ //avatar is always vertical
+ float r = chr.CapsuleRadius + chr2.CapsuleRadius;
+ float t = Math.Abs(chr2._position.X - chr._position.X);
+ if (t > r)
+ continue;
+ t = Math.Abs(chr2._position.Y - chr._position.Y);
+ if (t > r)
+ continue;
+
+ r = chr.CapsuleSizeZ + chr2.CapsuleSizeZ;
+ t = Math.Abs(chr2._position.Z - chr._position.Z);
+ if (t > r)
+ continue;
+ */
+ CollideCharChar(chr, chr2);
+ }
+ }
+ }
+ }
+
// chars with chars
//SafeNativeMethods.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
}
@@ -1037,8 +1177,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if(!aprim.m_outbounds && SafeNativeMethods.BodyIsEnabled(aprim.Body) &&
aprim.m_collide_geom != IntPtr.Zero)
{
- SafeNativeMethods.SpaceCollide2(StaticSpace, aprim.m_collide_geom, IntPtr.Zero, nearCallback);
- SafeNativeMethods.SpaceCollide2(TerrainGeom, aprim.m_collide_geom, IntPtr.Zero, nearCallback);
+ SafeNativeMethods.SpaceCollide2(StaticSpace, aprim.m_collide_geom, IntPtr.Zero, DefaultNearCallback);
+ SafeNativeMethods.SpaceCollide2(TerrainGeom, aprim.m_collide_geom, IntPtr.Zero, DefaultNearCallback);
}
}
}
@@ -1051,7 +1191,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// colide active amoung them
try
{
- SafeNativeMethods.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
+ SafeNativeMethods.SpaceCollide(ActiveSpace, IntPtr.Zero, DefaultNearCallback);
}
catch (Exception e)
{
@@ -1120,7 +1260,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
lock (_characters)
{
- _characters.Add(chr);
+ if (_characters.Add(chr))
+ {
+ chr._charsListIndex = _charactersList.Count;
+ _charactersList.Add(chr);
+ }
+ else
+ chr._charsListIndex = -1;
if (chr.bad)
m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.LocalID);
}
@@ -1130,7 +1276,17 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
lock (_characters)
{
- _characters.Remove(chr);
+ if (_characters.Remove(chr))
+ {
+ if (chr._charsListIndex >= 0)
+ {
+ int last = _charactersList.Count - 1;
+ if(chr._charsListIndex != last)
+ _charactersList[chr._charsListIndex] = _charactersList[last];
+ _charactersList.RemoveAt(last);
+ chr._charsListIndex = -1;
+ }
+ }
}
}
@@ -1167,6 +1323,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
{
@@ -1196,6 +1353,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void remActivePrim(OdePrim deactivatePrim)
{
lock (_activeprims)
@@ -1204,6 +1362,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void remActiveGroup(OdePrim deactivatePrim)
{
lock (_activegroups)
@@ -1212,6 +1371,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public override void RemovePrim(PhysicsActor prim)
{
// As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
@@ -1223,6 +1383,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void RemovePrimThreadLocked(OdePrim prim)
{
//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
@@ -1230,12 +1391,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
_prims.Remove(prim.LocalID);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void addToPrims(OdePrim prim)
{
lock (_prims)
_prims[prim.LocalID] = prim;
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public OdePrim getPrim(uint id)
{
lock (_prims)
@@ -1247,12 +1410,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public bool havePrim(OdePrim prm)
{
lock (_prims)
return _prims.ContainsKey(prm.LocalID);
}
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
public void changePrimID(OdePrim prim,uint oldID)
{
lock (_prims)
@@ -1508,7 +1673,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
try
{
// clear pointer/counter to contacts to pass into joints
- m_global_contactcount = 0;
+ ContactJointCount = 0;
//tmpTime = Util.GetTimeStampMS();
@@ -1591,7 +1756,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
lock (SimulationLock)
{
SafeNativeMethods.WorldQuickStep(world, ODE_STEPSIZE);
- SafeNativeMethods.JointGroupEmpty(contactgroup);
+ SafeNativeMethods.JointGroupEmpty(JointContactGroup);
}
//qstepTIme += Util.GetTimeStampMS() - tmpTime;