ogb912/opensim
1
0
Fork 0
mirror of https://github.com/opensim/opensim.git synced 2026-08-07 01:35:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

a few changtes to ubode

Browse Source
This commit is contained in:
UbitUmarov
2022-10-19 23:22:21 +01:00
parent 7a3ff3b210
commit cd037cd2ea
4 changed files with 391 additions and 303 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs
View File

@@ -242,12 +242,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
internal SurfaceParameters surface;
internal ContactGeom geom;
internal Vector3 fdir1;
internal static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
}
internal static readonly int SizeOfContact = Marshal.SizeOf(typeof(Contact));
[StructLayout(LayoutKind.Sequential)]
//needed for Contact only, rest should use the class
internal struct ContactGeom
{
@@ -258,20 +257,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
internal IntPtr g2;
internal int side1;
internal int side2;
internal static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
}
/*
[StructLayout(LayoutKind.Sequential)]
internal struct GeomClass
{
internal int bytes;
internal GetColliderFnFn collider;
internal GetAABBFn aabb;
internal AABBTestFn aabb_test;
internal GeomDtorFn dtor;
}
*/
internal static readonly int SizeOfContactGeom = Marshal.SizeOf(typeof(ContactGeom));
[StructLayout(LayoutKind.Sequential)]
internal struct JointFeedback
@@ -541,7 +529,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
unsafe
{
Vector3* vtmp = BodyGetAngularVelUnsafe(body);
return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z);
return Unsafe.As<SafeNativeMethods.Vector3, OMV.Vector3>(ref *vtmp);
}
}
@@ -551,7 +539,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
unsafe
{
Vector3* vtmp = BodyGetAngularVelUnsafe(body);
return new OMV.Vector3(0, 0, (float)Math.Round(vtmp->Z, 3));
return Unsafe.As<SafeNativeMethods.Vector3, OMV.Vector3>(ref *vtmp);
}
}
@@ -593,7 +581,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
unsafe
{
Vector3* vtmp = BodyGetLinearVelUnsafe(body);
return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z);
return Unsafe.As<SafeNativeMethods.Vector3, OMV.Vector3>(ref *vtmp);
}
}
@@ -619,7 +607,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
unsafe
{
Vector3* vtmp = BodyGetPositionUnsafe(body);
return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z);
return Unsafe.As<SafeNativeMethods.Vector3, OMV.Vector3>(ref *vtmp);
}
}
@@ -1076,7 +1064,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
unsafe
{
Vector3* vtmp = GeomGetPositionUnsafe(geom);
return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z);
return Unsafe.As<SafeNativeMethods.Vector3, OMV.Vector3>(ref *vtmp);
}
}
@@ -1142,8 +1130,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
dReal width, dReal depth, int widthSamples, int depthSamples,
dReal scale, dReal offset, dReal thickness, int bWrap);
[DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
internal static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
dReal width, dReal depth, int widthSamples, int depthSamples,
@@ -1169,7 +1155,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
[DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
internal static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
[DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
internal static extern void GeomOSTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData,
dReal sampleSize, int widthSamples, int depthSamples,

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

@@ -1028,7 +1028,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
//in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
public static void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
{
float sin = 2.0f * rot.Z * rot.W;
float cos = rot.W * rot.W - rot.Z * rot.Z;
@@ -1037,20 +1037,20 @@ namespace OpenSim.Region.PhysicsModule.ubOde
x = tx * cos - y * sin;
y = tx * sin + y * cos;
}
public void RotateXYonZ(ref float x, ref float y, float sin, float cos)
public static void RotateXYonZ(ref float x, ref float y, float sin, float cos)
{
float tx = x;
x = tx * cos - y * sin;
y = tx * sin + y * cos;
}
public void invRotateXYonZ(ref float x, ref float y, float sin, float cos)
public static void invRotateXYonZ(ref float x, ref float y, float sin, float cos)
{
float tx = x;
x = tx * cos + y * sin;
y = -tx * sin + y * cos;
}
public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
public static void invRotateXYonZ(ref float x, ref float y, in Quaternion rot)
{
float sin = -2.0f * rot.Z * rot.W;
float cos = rot.W * rot.W - rot.Z * rot.Z;
@@ -1061,129 +1061,116 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
internal bool Collide(IntPtr me, IntPtr other, bool reverse, ref SafeNativeMethods.ContactGeom contact,
internal bool Collide(IntPtr other, bool reverse, ref SafeNativeMethods.ContactGeom contact,
ref SafeNativeMethods.ContactGeom altContact, ref bool useAltcontact, ref bool feetcollision)
{
feetcollision = false;
useAltcontact = false;
//if (me == capsule)
if (me == collider)
Vector3 offset;
float h = contact.pos.Z - _position.Z;
offset.Z = h - feetOff;
offset.X = contact.pos.X - _position.X;
offset.Y = contact.pos.Y - _position.Y;
SafeNativeMethods.GeomClassID gtype = SafeNativeMethods.GeomGetClass(other);
if (gtype == SafeNativeMethods.GeomClassID.CapsuleClass)
{
Vector3 offset;
Vector3 roff = offset * Quaternion.Inverse(m_orientation2D);
float r = roff.X * roff.X / AvaAvaSizeXsq;
r += (roff.Y * roff.Y) / AvaAvaSizeYsq;
if (r > 1.0f)
return false;
float h = contact.pos.Z - _position.Z;
offset.Z = h - feetOff;
float dp = 1.0f - MathF.Sqrt(r);
if (dp > 0.05f)
dp = 0.05f;
offset.X = contact.pos.X - _position.X;
offset.Y = contact.pos.Y - _position.Y;
contact.depth = dp;
SafeNativeMethods.GeomClassID gtype = SafeNativeMethods.GeomGetClass(other);
if (gtype == SafeNativeMethods.GeomClassID.CapsuleClass)
if (offset.Z < 0)
{
Vector3 roff = offset * Quaternion.Inverse(m_orientation2D);
float r = roff.X * roff.X / AvaAvaSizeXsq;
r += (roff.Y * roff.Y) / AvaAvaSizeYsq;
if (r > 1.0f)
return false;
float dp = 1.0f - MathF.Sqrt(r);
if (dp > 0.05f)
dp = 0.05f;
contact.depth = dp;
if (offset.Z < 0)
feetcollision = true;
if (h < boneOff)
{
feetcollision = true;
if (h < boneOff)
{
m_collideNormal.X = contact.normal.X;
m_collideNormal.Y = contact.normal.Y;
m_collideNormal.Z = contact.normal.Z;
IsColliding = true;
}
m_collideNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref contact.normal);
IsColliding = true;
}
return true;
}
if (gtype == SafeNativeMethods.GeomClassID.SphereClass && SafeNativeMethods.GeomGetBody(other) != IntPtr.Zero)
{
if (SafeNativeMethods.GeomSphereGetRadius(other) < 0.5)
return true;
}
if (offset.Z > 0 || contact.normal.Z > 0.35f)
{
if (offset.Z <= 0)
{
feetcollision = true;
if (h < boneOff)
{
m_collideNormal.X = contact.normal.X;
m_collideNormal.Y = contact.normal.Y;
m_collideNormal.Z = contact.normal.Z;
IsColliding = true;
}
}
return true;
}
if (m_flying)
return true;
feetcollision = true;
if (h < boneOff)
{
m_collideNormal.X = contact.normal.X;
m_collideNormal.Y = contact.normal.Y;
m_collideNormal.Z = contact.normal.Z;
IsColliding = true;
}
useAltcontact = true;
offset.Z -= 0.2f;
offset.Normalize();
float tdp = contact.depth;
float t = offset.X;
t = MathF.Abs(t);
if (t > 1e-6)
{
tdp /= t;
tdp *= contact.normal.X;
}
else
tdp *= 10;
if (tdp > 0.25f)
tdp = 0.25f;
altContact.pos = contact.pos;
//altContact.g1 = contact.g1;
//altContact.g2 = contact.g2;
//altContact.side1 = contact.side1;
//altContact.side2 = contact.side2;
altContact.depth = tdp;
if (reverse)
{
altContact.normal.X = offset.X;
altContact.normal.Y = offset.Y;
altContact.normal.Z = offset.Z;
}
else
{
altContact.normal.X = -offset.X;
altContact.normal.Y = -offset.Y;
altContact.normal.Z = -offset.Z;
}
return true;
}
return false;
if (gtype == SafeNativeMethods.GeomClassID.SphereClass && SafeNativeMethods.GeomGetBody(other) != IntPtr.Zero)
{
if (SafeNativeMethods.GeomSphereGetRadius(other) < 0.5)
return true;
}
if (offset.Z > 0 || contact.normal.Z > 0.35f)
{
if (offset.Z <= 0)
{
feetcollision = true;
if (h < boneOff)
{
m_collideNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref contact.normal);
IsColliding = true;
}
}
return true;
}
if (m_flying)
return true;
feetcollision = true;
if (h < boneOff)
{
m_collideNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref contact.normal);
IsColliding = true;
}
useAltcontact = true;
offset.Z -= 0.2f;
offset.Normalize();
float tdp = contact.depth;
float t = offset.X;
t = MathF.Abs(t);
if (t > 1e-6)
{
tdp /= t;
tdp *= contact.normal.X;
}
else
tdp *= 10;
if (tdp > 0.25f)
tdp = 0.25f;
altContact.pos = contact.pos;
//altContact.g1 = contact.g1;
//altContact.g2 = contact.g2;
//altContact.side1 = contact.side1;
//altContact.side2 = contact.side2;
altContact.depth = tdp;
if (reverse)
{
altContact.normal = Unsafe.As<Vector3, SafeNativeMethods.Vector3>(ref offset);
}
else
{
altContact.normal.X = -offset.X;
altContact.normal.Y = -offset.Y;
altContact.normal.Z = -offset.Z;
}
return true;
}
/// <summary>

244
OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs
View File

@@ -27,6 +27,7 @@
using System;
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Reflection;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
@@ -47,7 +48,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
/// <summary>
/// Pending ray requests
/// </summary>
protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
protected ConcurrentQueue<ODERayRequest> m_PendingRequests = new();
/// <summary>
/// Scene that created this object.
@@ -58,7 +59,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
IntPtr ray; // the ray. we only need one for our lifetime
private int CollisionContactGeomsPerTest = 25;
private const int DefaultMaxCount = 25;
private const int ResultsMaxCount = 25;
private const int DefaultResultsMaxCount = 25;
private const int MaxTimePerCallMS = 30;
/// <summary>
@@ -66,10 +68,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
/// </summary>
private readonly SafeNativeMethods.NearCallback nearCallback;
private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
private readonly List<ContactResult> m_contactResults = new List<ContactResult>();
private readonly List<ContactResult> m_contactResults = new(ResultsMaxCount);
private readonly object m_contactResultsLock = new();
private RayFilterFlags CurrentRayFilter;
private int CurrentMaxCount;
ContactResult SharedCollisionResult = new ContactResult();
ContactResult SharedCollisionResult = new();
public ODERayCastRequestManager(ODEScene pScene)
{
@@ -84,7 +87,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public void QueueRequest(ODERayRequest req)
{
if (req.Count == 0)
req.Count = DefaultMaxCount;
req.Count = DefaultResultsMaxCount;
m_PendingRequests.Enqueue(req);
}
@@ -95,11 +98,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void ProcessQueuedRequests()
{
if (m_PendingRequests.Count == 0)
if (m_PendingRequests.IsEmpty)
return ;
int time = Util.EnvironmentTickCount();
while (m_PendingRequests.Dequeue(out ODERayRequest req))
while (m_PendingRequests.TryDequeue(out ODERayRequest req))
{
if(req.length <= 0)
{
@@ -112,10 +115,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
if (m_scene.haveActor(req.actor))
{
if (req.actor is OdePrim)
geom = ((OdePrim)req.actor).m_prim_geom;
else if (req.actor is OdeCharacter)
geom = ((OdeCharacter)req.actor).collider;
if (req.actor is OdePrim prim)
geom = prim.m_prim_geom;
else if (req.actor is OdeCharacter ch)
geom = ch.collider;
}
if (geom == IntPtr.Zero)
{
@@ -182,7 +185,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
break;
}
lock (m_contactResults)
lock (m_contactResultsLock)
m_contactResults.Clear();
}
/// <summary>
@@ -192,15 +195,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
///
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void NoContacts(ODERayRequest req)
private static void NoContacts(ODERayRequest req)
{
if (req.callbackMethod is RaycastCallback)
{
((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero);
return;
}
if (req.callbackMethod is RayCallback)
((RayCallback)req.callbackMethod)(new List<ContactResult>());
if (req.callbackMethod is RaycastCallback callback)
callback(false, Vector3.Zero, 0, 0, Vector3.Zero);
else if (req.callbackMethod is RayCallback raycallback)
raycallback(new List<ContactResult>());
}
private const RayFilterFlags FilterActiveSpace = RayFilterFlags.physical | RayFilterFlags.LSLPhantom;
@@ -259,11 +260,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
{
foreach(OdeCharacter chr in m_scene._charactersList)
foreach(OdeCharacter chr in CollectionsMarshal.AsSpan(m_scene._charactersList))
{
if (m_contactResults.Count >= CurrentMaxCount)
break;
collideRayAvatar(chr);
CollideRayAvatar(chr);
}
}
@@ -284,12 +285,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
tmp2 = (float)Math.Sqrt(tmp2);
SafeNativeMethods.GeomRaySetLength(ray, tmp2);
}
}
collideRayTerrain(m_scene.TerrainGeom);
CollideRayTerrain(m_scene.TerrainGeom);
}
if (req.callbackMethod is RaycastCallback)
if (req.callbackMethod is RaycastCallback callback)
{
// Define default results
bool hitYN = false;
@@ -299,7 +299,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
Vector3 snormal = Vector3.Zero;
// Find closest contact and object.
lock (m_contactResults)
lock (m_contactResultsLock)
{
foreach (ContactResult cResult in m_contactResults)
{
@@ -316,12 +316,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (distance > 0 && distance < float.MaxValue)
hitYN = true;
((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
callback(hitYN, closestcontact, hitConsumerID, distance, snormal);
}
else
{
List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
lock (m_PendingRequests)
List<ContactResult> cresult = new(m_contactResults.Count);
lock (m_contactResultsLock)
{
cresult.AddRange(m_contactResults);
m_contactResults.Clear();
@@ -343,7 +343,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// Collide test
SafeNativeMethods.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
if (req.callbackMethod is RaycastCallback)
if (req.callbackMethod is RaycastCallback callback)
{
// Define default results
bool hitYN = false;
@@ -353,7 +353,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
Vector3 snormal = Vector3.Zero;
// Find closest contact and object.
lock (m_contactResults)
lock (m_contactResultsLock)
{
foreach (ContactResult cResult in m_contactResults)
{
@@ -371,12 +371,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (distance > 0 && distance < float.MaxValue)
hitYN = true;
((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
callback(hitYN, closestcontact, hitConsumerID, distance, snormal);
}
else
{
List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
lock (m_PendingRequests)
List<ContactResult> cresult = new(m_contactResults.Count);
lock (m_contactResultsLock)
{
cresult.AddRange(m_contactResults);
m_contactResults.Clear();
@@ -407,10 +407,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
return;
}
int count = 0;
int count;
try
{
count = SafeNativeMethods.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
count = SafeNativeMethods.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom);
}
catch (Exception e)
{
@@ -423,52 +423,37 @@ namespace OpenSim.Region.PhysicsModule.ubOde
m_scene.actor_name_map.TryGetValue(g2, out PhysicsActor p2);
if (p2 == null)
if (p2 is null || p2.PhysicsActorType != (int)ActorTypes.Prim)
return;
uint ID = 0;
switch (p2.PhysicsActorType)
{
case (int)ActorTypes.Prim:
RayFilterFlags thisFlags = p2.IsPhysical ? RayFilterFlags.physical : RayFilterFlags.nonphysical;
RayFilterFlags thisFlags;
if (p2.Phantom)
thisFlags |= RayFilterFlags.phantom;
if (p2.IsPhysical)
thisFlags = RayFilterFlags.physical;
else
thisFlags = RayFilterFlags.nonphysical;
if (p2.IsVolumeDtc)
thisFlags |= RayFilterFlags.volumedtc;
if (p2.Phantom)
thisFlags |= RayFilterFlags.phantom;
if ((thisFlags & CurrentRayFilter) == 0)
return;
if (p2.IsVolumeDtc)
thisFlags |= RayFilterFlags.volumedtc;
if ((thisFlags & CurrentRayFilter) == 0)
return;
ID = ((OdePrim)p2).LocalID;
break;
default:
return;
// break;
}
uint ID = ((OdePrim)p2).LocalID;
// closestHit for now only works for meshs, so must do it for others
if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
{
// Loop all contacts, build results.
for (int i = 0; i < count; i++)
for (int i = 0; i < count; ++i)
{
lock (m_contactResults)
ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[i];
lock (m_contactResultsLock)
{
m_contactResults.Add(new ContactResult
{
ConsumerID = ID,
Pos = new Vector3(m_contacts[i].pos.X, m_contacts[i].pos.Y, m_contacts[i].pos.Z),
Normal = new Vector3(m_contacts[i].normal.X, m_contacts[i].normal.Y, m_contacts[i].normal.Z),
Depth = m_contacts[i].depth
Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos),
Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal),
Depth = curCtg.depth
});
if (m_contactResults.Count >= CurrentMaxCount)
return;
@@ -478,41 +463,37 @@ namespace OpenSim.Region.PhysicsModule.ubOde
else
{
// keep only closest contact
SharedCollisionResult.ConsumerID = ID;
SharedCollisionResult.Depth = float.MaxValue;
ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[0];
SharedCollisionResult.Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos);
SharedCollisionResult.Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal);
float depth = curCtg.depth;
SharedCollisionResult.Depth = depth;
for (int i = 0; i < count; i++)
for (int i = 1; i < count; ++i)
{
if (m_contacts[i].depth < SharedCollisionResult.Depth)
curCtg = ref m_contacts[i];
if (curCtg.depth < depth)
{
SharedCollisionResult.Pos.X = m_contacts[i].pos.X;
SharedCollisionResult.Pos.Y = m_contacts[i].pos.Y;
SharedCollisionResult.Pos.Z = m_contacts[i].pos.Z;
SharedCollisionResult.Normal.X = m_contacts[i].normal.X;
SharedCollisionResult.Normal.Y = m_contacts[i].normal.Y;
SharedCollisionResult.Normal.Z = m_contacts[i].normal.Z;
SharedCollisionResult.Depth = m_contacts[i].depth;
SharedCollisionResult.Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos);
SharedCollisionResult.Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal);
depth = curCtg.depth;
SharedCollisionResult.Depth = depth;
}
}
if (SharedCollisionResult.Depth != float.MaxValue)
{
lock (m_contactResults)
m_contactResults.Add(SharedCollisionResult);
}
SharedCollisionResult.ConsumerID = ID;
lock (m_contactResultsLock)
m_contactResults.Add(SharedCollisionResult);
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void collideRayAvatar(OdeCharacter chr)
private void CollideRayAvatar(OdeCharacter chr)
{
if (chr.collider == IntPtr.Zero)
return;
int count = 0;
int count;
try
{
count = SafeNativeMethods.CollidePtr(ray, chr.collider, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
count = SafeNativeMethods.CollidePtr(ray, chr.collider, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom);
if (count == 0)
return;
}
@@ -529,14 +510,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// Loop all contacts, build results.
for (int i = 0; i < count; i++)
{
lock (m_contactResults)
ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[i];
lock (m_contactResultsLock)
{
m_contactResults.Add(new ContactResult
{
ConsumerID = id,
Pos = new Vector3(m_contacts[i].pos.X, m_contacts[i].pos.Y, m_contacts[i].pos.Z),
Normal = new Vector3(m_contacts[i].normal.X, m_contacts[i].normal.Y, m_contacts[i].normal.Z),
Depth = m_contacts[i].depth
Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos),
Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal),
Depth = curCtg.depth
});
if (m_contactResults.Count >= CurrentMaxCount)
return;
@@ -546,41 +528,40 @@ namespace OpenSim.Region.PhysicsModule.ubOde
else
{
// keep only closest contact
SharedCollisionResult.ConsumerID = chr.LocalID;
SharedCollisionResult.Depth = float.MaxValue;
ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[0];
SharedCollisionResult.Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos);
SharedCollisionResult.Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal);
float depth = curCtg.depth;
SharedCollisionResult.Depth = depth;
for (int i = 0; i < count; i++)
for (int i = 1; i < count; ++i)
{
if (m_contacts[i].depth < SharedCollisionResult.Depth)
curCtg = ref m_contacts[i];
if (curCtg.depth < depth)
{
SharedCollisionResult.Pos.X = m_contacts[i].pos.X;
SharedCollisionResult.Pos.Y = m_contacts[i].pos.Y;
SharedCollisionResult.Pos.Z = m_contacts[i].pos.Z;
SharedCollisionResult.Normal.X = m_contacts[i].normal.X;
SharedCollisionResult.Normal.Y = m_contacts[i].normal.Y;
SharedCollisionResult.Normal.Z = m_contacts[i].normal.Z;
SharedCollisionResult.Depth = m_contacts[i].depth;
SharedCollisionResult.Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos);
SharedCollisionResult.Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal);
depth = curCtg.depth;
SharedCollisionResult.Depth = depth;
}
}
if (SharedCollisionResult.Depth != float.MaxValue)
{
lock (m_contactResults)
m_contactResults.Add(SharedCollisionResult);
}
SharedCollisionResult.ConsumerID = chr.LocalID;
lock (m_contactResultsLock)
m_contactResults.Add(SharedCollisionResult);
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void collideRayTerrain(IntPtr terrain)
private void CollideRayTerrain(IntPtr terrain)
{
if (terrain == IntPtr.Zero)
return;
int count = 0;
int count;
try
{
count = SafeNativeMethods.CollidePtr(ray, terrain, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
count = SafeNativeMethods.CollidePtr(ray, terrain, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom);
if (count == 0)
return;
}
@@ -596,14 +577,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// Loop all contacts, build results.
for (int i = 0; i < count; i++)
{
lock (m_contactResults)
ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[i];
lock (m_contactResultsLock)
{
m_contactResults.Add(new ContactResult
{
ConsumerID = 0,
Pos = new Vector3(m_contacts[i].pos.X, m_contacts[i].pos.Y, m_contacts[i].pos.Z),
Normal = new Vector3(m_contacts[i].normal.X, m_contacts[i].normal.Y, m_contacts[i].normal.Z),
Depth = m_contacts[i].depth
Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos),
Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal),
Depth = curCtg.depth
});
if (m_contactResults.Count >= CurrentMaxCount)
return;
@@ -613,28 +595,27 @@ namespace OpenSim.Region.PhysicsModule.ubOde
else
{
// keep only closest contact
SharedCollisionResult.ConsumerID = 0;
SharedCollisionResult.Depth = float.MaxValue;
ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[0];
SharedCollisionResult.Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos);
SharedCollisionResult.Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal);
float depth = curCtg.depth;
SharedCollisionResult.Depth = depth;
for (int i = 0; i < count; i++)
for (int i = 1; i < count; ++i)
{
if (m_contacts[i].depth < SharedCollisionResult.Depth)
curCtg = ref m_contacts[i];
if (curCtg.depth < depth)
{
SharedCollisionResult.Pos.X = m_contacts[i].pos.X;
SharedCollisionResult.Pos.Y = m_contacts[i].pos.Y;
SharedCollisionResult.Pos.Z = m_contacts[i].pos.Z;
SharedCollisionResult.Normal.X = m_contacts[i].normal.X;
SharedCollisionResult.Normal.Y = m_contacts[i].normal.Y;
SharedCollisionResult.Normal.Z = m_contacts[i].normal.Z;
SharedCollisionResult.Depth = m_contacts[i].depth;
SharedCollisionResult.Pos = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.pos);
SharedCollisionResult.Normal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curCtg.normal);
depth = curCtg.depth;
SharedCollisionResult.Depth = depth;
}
}
if (SharedCollisionResult.Depth != float.MaxValue)
{
lock (m_contactResults)
m_contactResults.Add(SharedCollisionResult);
}
SharedCollisionResult.ConsumerID = 0;
lock (m_contactResultsLock)
m_contactResults.Add(SharedCollisionResult);
}
}
@@ -651,7 +632,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
public class ODERayRequest
{
public Vector3 Origin;

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

@@ -474,7 +474,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
m_contactsHandler = GCHandle.Alloc(m_contacts, GCHandleType.Pinned);
ContactgeomsArray = m_contactsHandler.AddrOfPinnedObject();
GlobalContactsArray = Marshal.AllocHGlobal((maxContactJoints + 100) * SafeNativeMethods.Contact.unmanagedSizeOf);
GlobalContactsArray = Marshal.AllocHGlobal((maxContactJoints + 100) * SafeNativeMethods.SizeOfContact);
contactSharedForJoints.geom.g1 = IntPtr.Zero;
contactSharedForJoints.geom.g2 = IntPtr.Zero;
@@ -546,7 +546,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
contactSharedForJoints.geom.pos = contactGeom.pos;
contactSharedForJoints.geom.normal = contactGeom.normal;
IntPtr contact = new(GlobalContactsArray.ToInt64() + (Int64)(ContactJointCount * SafeNativeMethods.Contact.unmanagedSizeOf));
IntPtr contact = new(GlobalContactsArray.ToInt64() + (Int64)(ContactJointCount * SafeNativeMethods.SizeOfContact));
Marshal.StructureToPtr(contactSharedForJoints, contact, false);
return SafeNativeMethods.JointCreateContactPtr(world, JointContactGroup, contact);
}
@@ -578,7 +578,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)
{
@@ -598,10 +598,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
SafeNativeMethods.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
{
int cflags = unchecked((int)(1 | SafeNativeMethods.CONTACTS_UNIMPORTANT));
count = SafeNativeMethods.CollidePtr(g1, g2, cflags, ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
count = SafeNativeMethods.CollidePtr(g1, g2, cflags, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom);
}
else
count = SafeNativeMethods.CollidePtr(g1, g2, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
count = SafeNativeMethods.CollidePtr(g1, g2, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom);
}
catch (SEHException)
{
@@ -642,7 +642,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
curctc0.depth, false
);
collision_accounting_events(p1, p2, ref volDepthContact);
Collision_accounting_events(p1, p2, ref volDepthContact);
return;
}
@@ -786,7 +786,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (dop1ava)
{
if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
if ((((OdeCharacter)p1).Collide(g2, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
{
if (p2.PhysicsActorType == (int)ActorTypes.Agent)
{
@@ -801,7 +801,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
else if (dop2ava)
{
if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
if ((((OdeCharacter)p2).Collide(g1, true, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
{
if (p1.PhysicsActorType == (int)ActorTypes.Agent)
{
@@ -831,28 +831,169 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
maxDepth = curctc.depth;
maxDepthContact.PenetrationDepth = maxDepth;
maxDepthContact.Position.X = curctc.pos.X;
maxDepthContact.Position.Y = curctc.pos.Y;
maxDepthContact.Position.Z = curctc.pos.Z;
maxDepthContact.Position = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curctc.pos);
maxDepthContact.CharacterFeet = FeetCollision;
}
if (curctc.depth < minDepth)
{
minDepth = curctc.depth;
maxDepthContact.SurfaceNormal.X = curctc.normal.X;
maxDepthContact.SurfaceNormal.Y = curctc.normal.Y;
maxDepthContact.SurfaceNormal.Z = curctc.normal.Z;
maxDepthContact.SurfaceNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curctc.normal);
}
}
}
if (ncontacts > 0)
{
collision_accounting_events(p1, p2, ref maxDepthContact);
Collision_accounting_events(p1, p2, ref maxDepthContact);
}
}
private void CharPrimNearCallback(IntPtr space, IntPtr g1, IntPtr g2)
{
// no lock here! It's invoked from within Simulate(), which is thread-locked
if (ContactJointCount >= maxContactJoints)
return;
// Test if we're colliding a geom with a space.
// If so we have to drill down into the space recursively
if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
return;
//if (SafeNativeMethods.GeomIsSpace(g1) || SafeNativeMethods.GeomIsSpace(g2))
if (SafeNativeMethods.GeomIsSpace(g2))
{
// We'll be calling near recursivly if one
// of them is a space to find all of the
// contact points in the space
try
{
SafeNativeMethods.SpaceCollide2(g1, g2, IntPtr.Zero, CharPrimNearCallback);
}
catch (AccessViolationException)
{
m_log.Warn("[PHYSICS]: Unable to collide test a space");
}
return;
}
// Figure out how many contact points we have
int count = 0;
try
{
if (SafeNativeMethods.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
{
int cflags = unchecked((int)(1 | SafeNativeMethods.CONTACTS_UNIMPORTANT));
count = SafeNativeMethods.CollidePtr(g1, g2, cflags, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom);
}
else
count = SafeNativeMethods.CollidePtr(g1, g2, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom);
}
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;
// try get physical actors
if (!actor_name_map.TryGetValue(g1, out PhysicsActor p1))
{
m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
return;
}
if (!actor_name_map.TryGetValue(g2, out PhysicsActor p2))
{
m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
return;
}
// do volume detection case
if (p2.IsVolumeDtc)
{
ref SafeNativeMethods.ContactGeom curctc0 = ref m_contacts[0];
ContactPoint volDepthContact = new(
new Vector3(curctc0.pos.X, curctc0.pos.Y, curctc0.pos.Z),
new Vector3(curctc0.normal.X, curctc0.normal.Y, curctc0.normal.Z),
curctc0.depth, false
);
Collision_accounting_events(p1, p2, ref volDepthContact);
return;
}
if(p2.PhysicsActorType != (int)ActorTypes.Prim)
return;
IntPtr Joint;
bool FeetCollision = false;
int ncontacts = 0;
ContactPoint accountContact = new();
float minDepth = float.MaxValue;
float maxDepth = float.MinValue;
contactSharedForJoints.surface.mu = 0;
contactSharedForJoints.surface.bounce = 0;
bool useAltcontact;
IntPtr b1 = SafeNativeMethods.GeomGetBody(g1);
IntPtr b2 = SafeNativeMethods.GeomGetBody(g2);
for (int i = 0; i < count; ++i)
{
ref SafeNativeMethods.ContactGeom curctc = ref m_contacts[i];
useAltcontact = false;
if ((((OdeCharacter)p1).Collide(g2, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
{
if(p2.rootVelocity.LengthSquared() > 0.0f)
p2.CollidingObj = true;
Joint = useAltcontact ?
CreateContacJoint(ref altWorkContact, false) :
CreateContacJoint(ref curctc, false);
if (Joint == IntPtr.Zero)
break;
SafeNativeMethods.JointAttach(Joint, b1, b2);
ncontacts++;
if (curctc.depth > maxDepth)
{
maxDepth = curctc.depth;
accountContact.PenetrationDepth = maxDepth;
accountContact.Position = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curctc.pos);
accountContact.CharacterFeet = FeetCollision;
}
if (curctc.depth < minDepth)
{
minDepth = curctc.depth;
accountContact.SurfaceNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curctc.normal);
}
}
}
if (ncontacts > 0)
{
Collision_accounting_events(p1, p2, ref accountContact);
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void CollideCharChar(OdeCharacter p1, OdeCharacter p2)
{
@@ -863,7 +1004,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
int count = 0;
try
{
count = SafeNativeMethods.CollidePtr(p1.collider, p2.collider, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
count = SafeNativeMethods.CollidePtr(p1.collider, p2.collider, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom);
}
catch (SEHException)
{
@@ -885,7 +1026,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
bool FeetCollision = false;
int ncontacts = 0;
ContactPoint maxDepthContact = new();
ContactPoint maccountContact = new();
float minDepth = float.MaxValue;
float maxDepth = float.MinValue;
@@ -900,15 +1041,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
ref SafeNativeMethods.ContactGeom curctc = ref m_contacts[i];
useAltcontact = false;
if (p1.Collide(p1.collider, p2.collider, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision))
if (p1.Collide(p2.collider, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision))
{
p1.CollidingObj = true;
p1.CollidingObj = true;
if (useAltcontact)
Joint = CreateContacJoint(ref altWorkContact, false);
else
Joint = CreateContacJoint(ref curctc, false);
Joint = useAltcontact ?
CreateContacJoint(ref altWorkContact, false) :
CreateContacJoint(ref curctc, false);
if (Joint == IntPtr.Zero)
break;
@@ -919,30 +1059,26 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (curctc.depth > maxDepth)
{
maxDepth = curctc.depth;
maxDepthContact.PenetrationDepth = maxDepth;
maxDepthContact.Position.X = curctc.pos.X;
maxDepthContact.Position.Y = curctc.pos.Y;
maxDepthContact.Position.Z = curctc.pos.Z;
maxDepthContact.CharacterFeet = FeetCollision;
maccountContact.PenetrationDepth = maxDepth;
maccountContact.Position = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curctc.pos);
maccountContact.CharacterFeet = FeetCollision;
}
if (curctc.depth < minDepth)
{
minDepth = curctc.depth;
maxDepthContact.SurfaceNormal.X = curctc.normal.X;
maxDepthContact.SurfaceNormal.Y = curctc.normal.Y;
maxDepthContact.SurfaceNormal.Z = curctc.normal.Z;
maccountContact.SurfaceNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curctc.normal);
}
}
}
if (ncontacts > 0)
{
collision_accounting_events(p1, p2, ref maxDepthContact);
Collision_accounting_events(p1, p2, ref maccountContact);
}
}
private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ref ContactPoint contact)
private static void Collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ref ContactPoint contact)
{
// update actors collision score
@@ -1054,8 +1190,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (chr.Body != IntPtr.Zero && chr.collider != IntPtr.Zero)
{
SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, NearCallback);
SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, NearCallback);
SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, CharPrimNearCallback);
SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, CharPrimNearCallback);
}
}
}
@@ -1073,8 +1209,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
chr.CollidingObj = false;
// do colisions with static space
SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, NearCallback);
SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, NearCallback);
SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, CharPrimNearCallback);
SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, CharPrimNearCallback);
float mx = chr._AABB2D.minx;
float Mx = chr._AABB2D.maxx;