ogb912/opensim
1
0
Fork 0
mirror of https://github.com/opensim/opensim.git synced 2026-08-05 08:55:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

ubode: do part of avatar/avatar broadband collision phase in managed code plus cosmetics

Browse Source
This commit is contained in:
UbitUmarov
2022-08-14 19:05:14 +01:00
parent aad0c3c60e
commit b763428ede
4 changed files with 434 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
View File

@@ -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) { }

4
OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs
View File

@@ -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);

175
OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs
View File

@@ -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
/// </summary>
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
/// </summary>
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
/// </summary>
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
/// </summary>
/// <param name="status"></param>
[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
/// </summary>
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
/// </summary>
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
/// </summary>
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
/// </summary>
/// <param name="force"></param>
[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
/// <summary>
/// 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
/// <summary>
/// Cleanup the things we use in the scene.
/// </summary>
[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;
}
}
}

391
OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
View File

@@ -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<uint, OdePrim> _prims = new Dictionary<uint, OdePrim>();
private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
private readonly HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
public List<OdeCharacter> _charactersList;
public readonly ConcurrentQueue<ODEchangeitem> ChangesQueue = new ConcurrentQueue<ODEchangeitem>();
@@ -235,7 +235,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
private readonly List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
private readonly List<OdeCharacter> _badCharacter = new List<OdeCharacter>();
public readonly Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
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<OdeCharacter>(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();
/// <summary>
/// This is our near callback. A geometry is near a body
/// </summary>
@@ -574,11 +575,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
/// <param name="g1">a geometry or space</param>
/// <param name="g2">another geometry or space</param>
///
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;