ogb912/opensim
1
0
Fork 0
mirror of https://github.com/opensim/opensim.git synced 2026-08-10 11:33:28 +08:00
Code Issues Packages Projects Releases Wiki Activity

ubode do ray-avatar collision in managed space

Browse Source
This commit is contained in:
UbitUmarov
2022-11-08 03:51:30 +00:00
parent 9e58f0f489
commit 87cf092666
3 changed files with 157 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

166
OpenSim/Region/PhysicsModules/ubOde/ODECollision.cs
View File

@@ -1,16 +1,7 @@
using System;
using System.Collections.Generic;
using System.Reflection;
using log4net;
using Nini.Config;
using Mono.Addins;
using OpenSim.Framework;
using OpenSim.Region.Framework.Scenes;
using OpenSim.Region.Framework.Interfaces;
using OpenMetaverse;
using System.Runtime.CompilerServices;
using System.Runtime.ConstrainedExecution;
using OpenMetaverse;
using OpenSim.Region.PhysicsModules.SharedBase;
using System;
using System.Runtime.CompilerServices;
namespace OpenSim.Region.PhysicsModule.ubOde
{
@@ -75,18 +66,19 @@ namespace OpenSim.Region.PhysicsModule.ubOde
r -= rsum;
if (absdz < r)
{
cp.Position = p1._position;
cp.Position.Z -= 0.5f * tt.Z;
float hd = tt.X * tt.X + tt.Y * tt.Y;
if (hd < 1e-6f)
{
cp.Position = p1._position;
cp.Position.Z -= 0.5f * tt.Z;
cp.SurfaceNormal = minusXRotateByShortQZ(p1.Orientation2D);
cp.PenetrationDepth = rsum;
cp.PenetrationDepth = rsum + .05f;
return true;
}
hd = MathF.Sqrt(hd);
if(hd > rsum)
return false;
d = hd;
hd = 1.0f / hd;
@@ -106,6 +98,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
cp.SurfaceNormal.Y = tt.Y;
cp.SurfaceNormal.Z = 0;
cp.Position = p1._position;
cp.Position.Z -= 0.5f * tt.Z;
float hk = r1 - 0.5f * rhsum;
if (hk <= 0)
cp.PenetrationDepth = r1;
@@ -142,7 +137,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
d = MathF.Sqrt(d);
float k = rsum - d;
if (k < 0.5e-3f)
if (k < 0.5e-4f)
return false;
tt *= 1.0f / d;
@@ -158,7 +153,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
cp.SurfaceNormal = tt;
cp.PenetrationDepth = k;
k = p1.CapsuleRadius - 0.5f * k;
k = p1.CapsuleRadius - 0.5f * k;
tt *= k;
cp.Position = p1._position - tt;
return true;
@@ -229,6 +224,141 @@ namespace OpenSim.Region.PhysicsModule.ubOde
p1.AddCollisionEvent(p2.ParentActor.m_baseLocalID, SharedChrContact);
}
}
// if mode==1 then use the sphere exit contact, not the entry contact
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private static bool ray_sphere_helper(Vector3 pos, Vector3 dir, float len, Vector3 sphere_pos, float radius,
ref ContactResult cp)
{
Vector3 q = pos - sphere_pos;
float B = q.Dot(dir);
float C = q.LengthSquared() - radius * radius;
// note: if C <= 0 then the start of the ray is inside the sphere
float k = B * B - C;
if (k < 1e-6f)
return false;
k = MathF.Sqrt(k);
float alpha;
if (C >= 0)
{
alpha = -B + k;
if (alpha < 0)
return false;
}
else
{
alpha = -B - k;
if (alpha < 0)
{
alpha = -B + k;
if (alpha < 0)
return false;
}
}
if (alpha > len)
return false;
cp.Pos = pos + dir * alpha;
cp.Normal = sphere_pos - cp.Pos;
cp.Normal.Normalize();
cp.Depth = alpha;
return true;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
internal bool CollideRaySimpleCapsule(OdeCharacter p1, Vector3 RayStart, Vector3 RayDirection, float RayLength,
ref ContactResult cp)
{
Vector3 cs;
cs.Z = RayStart.Z - p1._position.Z;
float distZ = Math.Abs(cs.Z) - p1.CapsuleSizeZ;
if (distZ >= RayLength)
return false;
cs.X = RayStart.X - p1._position.X;
cs.Y = RayStart.Y - p1._position.Y;
float C = cs.X * cs.X + cs.Y * cs.Y - p1.CapsuleRadius * p1.CapsuleRadius;
if(C > RayLength * RayLength)
return false;
if (C <= 0.5e-4f)
{
if (distZ < 0.5e-4f)
return false;
else
{
cp.Pos.X = p1._position.X;
cp.Pos.Y = p1._position.Y;
cp.Normal.X = 0;
cp.Normal.Y = 0;
cp.Depth = distZ;
if (cs.Z < 0)
{
cp.Pos.Z = p1._position.Z - p1.CapsuleSizeZ;
cp.Normal.Z = -1f;
}
else
{
cp.Pos.Z = p1._position.Z + p1.CapsuleSizeZ;
cp.Normal.Z = 1f;
}
return true;
}
}
float k;
float lz2 = p1.CapsuleSizeZ - p1.CapsuleRadius;
// compute ray collision with infinite cylinder, except for the case where
// the ray is outside the capped cylinder but within the infinite cylinder
// (it that case the ray can only hit endcaps)
float A = RayDirection.X * RayDirection.X + RayDirection.Y * RayDirection.Y;
// A == 0 means that the ray and ccylinder axes are parallel
if (A == 0)
goto _checkends;
float B = 2f * (cs.X * RayDirection.X + cs.Y * RayDirection.Y);
k = B * B - 4f * A * C;
if (k < 1e-6f)
return false;
k = MathF.Sqrt(k);
A = 0.5f / A;
float alpha = (-B - k) * A;
if (alpha < 0)
{
alpha = (-B + k) * A;
if (alpha < 0)
return false;
}
if (alpha > RayLength)
return false;
// the ray intersects the infinite cylinder. check to see if the
// intersection point is between the caps
cp.Pos = RayStart + RayDirection * alpha;
Vector3 q = cp.Pos - p1._position;
if (q.Z >= -lz2 && q.Z <= lz2)
{
cp.Normal.X = q.X;
cp.Normal.Y = q.Y;
cp.Normal.Z = 0;
cp.Normal.Normalize();
cp.Depth = alpha;
return true;
}
// the infinite cylinder intersection point is not between the caps.
// set k to cap position to check.
_checkends:
k = cs.Z < 0 ? -lz2 : lz2;
return ray_sphere_helper(RayStart, RayDirection, RayLength,
new(p1._position.X, p1._position.Y, p1._position.Z + k),
p1.CapsuleRadius, ref cp);
}
}
}

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

@@ -210,47 +210,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void doSpaceRay(ODERayRequest req)
{
/*
float endx;
float endy;
if (req.Normal.X <= 0)
{
if (req.Origin.X <= 0)
{
NoContacts(req);
return;
}
endx = req.Origin.X - req.length;
if (endx < 0)
{
req.length += endx;
endx = 0;
}
}
else
{
if (req.Origin.X < 0)
{
req.length += req.Origin.X;
if (req.length <= 0)
{
NoContacts(req);
return;
}
req.Origin.X = 0;
}
endx = req.Origin.X + req.length;
if (endx < 0)
{
req.length += endx;
endx = 0;
}
}
*/
SafeNativeMethods.GeomRaySetLength(ray, req.length);
SafeNativeMethods.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
@@ -264,7 +223,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
if (m_contactResults.Count >= CurrentMaxCount)
break;
CollideRayAvatar(chr);
if(m_scene.CollideRaySimpleCapsule(chr, req.Origin, req.Normal, req.length, ref SharedCollisionResult))
{
SharedCollisionResult.ConsumerID = chr.m_baseLocalID;
m_contactResults.Add(SharedCollisionResult);
}
}
}

6
OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
View File

@@ -15903,7 +15903,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
if (rejectHostGroup && part.ParentGroup == thisgrp)
continue;
if ((dataFlags & ScriptBaseClass.RC_GET_ROOT_KEY) == ScriptBaseClass.RC_GET_ROOT_KEY)
if ((dataFlags & ScriptBaseClass.RC_GET_ROOT_KEY) != 0)
itemID = part.ParentGroup.UUID;
else
itemID = part.UUID;
@@ -15921,10 +15921,10 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
list.Add(new LSL_String(itemID.ToString()));
list.Add(new LSL_String(result.Pos.ToString()));
if ((dataFlags & ScriptBaseClass.RC_GET_LINK_NUM) == ScriptBaseClass.RC_GET_LINK_NUM)
if ((dataFlags & ScriptBaseClass.RC_GET_LINK_NUM) != 0)
list.Add(new LSL_Integer(linkNum));
if ((dataFlags & ScriptBaseClass.RC_GET_NORMAL) == ScriptBaseClass.RC_GET_NORMAL)
if ((dataFlags & ScriptBaseClass.RC_GET_NORMAL) != 0)
list.Add(new LSL_Vector(result.Normal));
values++;