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

ubode: several changes to avatar-avatar collisions. Replace some unmanaged by managed code

Browse Source
This commit is contained in:
UbitUmarov
2022-11-04 21:45:44 +00:00
parent fd6edf81d9
commit 3931400d7f
3 changed files with 366 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -78,16 +78,17 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private Vector3 _acceleration;
private Vector3 m_rotationalVelocity;
private Vector3 m_size;
private Vector3 m_collideNormal;
private Vector3 m_lastFallVel;
private Quaternion m_orientation;
private Quaternion m_orientation2D;
internal Quaternion m_orientation;
internal Vector2 Orientation2D;
private SafeNativeMethods.Quaternion m_NativeOrientation2D;
private float m_mass = 80f;
private float m_massInvTimeScaled = 1600f;
public readonly float m_density = 60f;
private bool m_pidControllerActive = true;
internal Vector3 CollideNormal;
public float CapsuleRadius;
public float CapsuleSizeZ;
@@ -107,11 +108,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private readonly float m_sceneTimeStep;
private readonly float m_sceneInverseTimeStep;
private float m_feetOffset = 0;
private float feetOff = 0;
private float boneOff = 0;
private float AvaAvaSizeXsq = 0.3f;
private float AvaAvaSizeYsq = 0.2f;
internal float feetOff = 0;
internal float boneOff = 0;
internal float AvaSizeXsq = 0.3f;
internal float AvaSizeYsq = 0.2f;
public readonly float m_walkMultiplier = 1.0f / 1.3f;
public readonly float m_runMultiplier = 1.0f / 0.8f;
@@ -196,23 +196,24 @@ namespace OpenSim.Region.PhysicsModule.ubOde
m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
m_size.X = pSize.X > 0.01f ? pSize.X : 0.01f;
m_size.Y = pSize.Y > 0.01f ? pSize.Y : 0.01f;
m_size.Z = pSize.Z > 0.01f ? pSize.Z : 0.01f;
m_size.X = pSize.X > 0.01f ? 0.5f * pSize.X : 0.01f;
m_size.Y = pSize.Y > 0.01f ? 0.5f * pSize.Y : 0.01f;
m_size.Z = pSize.Z > 0.01f ? 0.5f * 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;
CapsuleRadius = MathF.Max(m_size.X, m_size.Y);
CapsuleSizeZ = m_size.Z;
AvaSizeXsq = m_size.X;
AvaSizeXsq *= AvaSizeXsq;
AvaSizeYsq = m_size.Y;
AvaSizeYsq *= AvaSizeYsq;
m_feetOffset = pfeetOffset;
m_orientation = Quaternion.Identity;
m_orientation2D = Quaternion.Identity;
m_NativeOrientation2D.X = 0;
m_NativeOrientation2D.Y = 0;
m_NativeOrientation2D.Z = 0;
m_NativeOrientation2D.W = 1;
Orientation2D = new(0f, 1f);
m_NativeOrientation2D.X = 0f;
m_NativeOrientation2D.Y = 0f;
m_NativeOrientation2D.Z = 0f;
m_NativeOrientation2D.W = 1f;
m_density = density;
@@ -224,7 +225,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
m_walkMultiplier = 1.0f / walk_divisor;
m_runMultiplier = 1.0f / rundivisor;
m_mass = m_density * m_size.X * m_size.Y * m_size.Z;
m_mass = 8f * m_density * m_size.X * m_size.Y * m_size.Z;
m_massInvTimeScaled = m_mass * m_sceneInverseTimeStep;
// sure we have a default
@@ -567,7 +568,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
[MethodImpl(MethodImplOptions.AggressiveInlining)]
get
{
return m_size;
return m_size * 2f;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
set
@@ -605,7 +606,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
strAvatarSize st = new()
{
size = size,
offset = feetOffset
};
AddChange(changes.AvatarSize, st);
}
@@ -893,7 +893,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
float sy = m_size.Y;
float sz = m_size.Z;
float bot = -sz * 0.5f + m_feetOffset;
float bot = -sz;
boneOff = bot + 0.3f;
float feetsz = sz * 0.45f;
@@ -901,25 +901,22 @@ namespace OpenSim.Region.PhysicsModule.ubOde
feetsz = 0.6f;
feetOff = bot + feetsz;
AvaAvaSizeXsq = 0.4f * sx;
AvaAvaSizeXsq *= AvaAvaSizeXsq;
AvaAvaSizeYsq = 0.5f * sy;
AvaAvaSizeYsq *= AvaAvaSizeYsq;
AvaSizeXsq = sx;
AvaSizeXsq *= AvaSizeXsq;
AvaSizeYsq = sy;
AvaSizeYsq *= AvaSizeYsq;
CapsuleRadius = sx;
if (sy > CapsuleRadius)
CapsuleRadius = sy;
CapsuleRadius = MathF.Max(sx, sy);
float l = sz - CapsuleRadius;
CapsuleRadius *= 0.5f;
CapsuleSizeZ = 0.5f * sz;
CapsuleSizeZ = sz;
collider = SafeNativeMethods.CreateCapsule(m_parent_scene.TopSpace, CapsuleRadius, l);
collider = SafeNativeMethods.CreateCapsule(m_parent_scene.TopSpace, CapsuleRadius, 2.0f * l);
SafeNativeMethods.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
SafeNativeMethods.GeomSetCollideBits(collider, (uint)m_collisionFlags);
// update mass
m_mass = m_density * sx * sy * sz;
SafeNativeMethods.MassSetBoxTotal(out ShellMass, m_mass, sx, sy, sz);
m_mass = 8f * m_density * sx * sy * sz;
SafeNativeMethods.MassSetBoxTotal(out ShellMass, m_mass, 2f * sx, 2f * sy, 2f * sz);
m_massInvTimeScaled = m_mass * m_sceneInverseTimeStep;
PID_D = basePID_D * m_massInvTimeScaled;
PID_P = basePID_P * m_massInvTimeScaled;
@@ -1076,31 +1073,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
offset.Y = contact.pos.Y - _position.Y;
SafeNativeMethods.GeomClassID gtype = SafeNativeMethods.GeomGetClass(other);
if (gtype == SafeNativeMethods.GeomClassID.CapsuleClass)
{
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)
{
m_collideNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref contact.normal);
IsColliding = true;
}
}
return true;
}
if (gtype == SafeNativeMethods.GeomClassID.SphereClass && SafeNativeMethods.GeomGetBody(other) != IntPtr.Zero)
{
@@ -1115,7 +1087,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
feetcollision = true;
if (h < boneOff)
{
m_collideNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref contact.normal);
CollideNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref contact.normal);
IsColliding = true;
}
}
@@ -1128,7 +1100,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
feetcollision = true;
if (h < boneOff)
{
m_collideNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref contact.normal);
CollideNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref contact.normal);
IsColliding = true;
}
@@ -1153,11 +1125,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
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)
@@ -1248,10 +1215,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
SafeNativeMethods.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
}
if (m_pidControllerActive == false)
{
if (!m_pidControllerActive)
_zeroPosition = _position;
}
//Update AABB
UpdateAABB2D();
@@ -1259,16 +1224,25 @@ namespace OpenSim.Region.PhysicsModule.ubOde
float aabbminz = _position.Z - CapsuleSizeZ;
//float aabbmaxz = _position.Z + CapsuleSizeZ;
Vector3 ctz = m_targetVelocity;
if (m_alwaysRun)
{
ctz.X *= m_runMultiplier;
ctz.Y *= m_runMultiplier;
}
bool tviszero = m_targetVelocity.IsZero();
Vector3 ctv;
if (tviszero)
ctv = Vector3.Zero;
else
{
ctz.X *= m_walkMultiplier;
ctz.Y *= m_walkMultiplier;
if (m_alwaysRun)
{
ctv = new( m_targetVelocity.X * m_runMultiplier,
m_targetVelocity.Y * m_runMultiplier,
m_targetVelocity.Z);
}
else
{
ctv = new ( m_targetVelocity.X * m_walkMultiplier,
m_targetVelocity.Y * m_walkMultiplier,
m_targetVelocity.Z);
}
}
Vector3 vel = SafeNativeMethods.BodyGetLinearVelOMV(Body);
@@ -1287,8 +1261,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
float terrainheight = m_parent_scene.GetTerrainHeightAtXY(tmpX, tmpY);
if (aabbminz < terrainheight)
{
if (ctz.Z < 0f)
ctz.Z = 0f;
if (ctv.Z < 0f)
ctv.Z = 0f;
if (!m_haveLastFallVel)
{
@@ -1335,7 +1309,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
m_freemove = false;
}
m_collideNormal = n;
CollideNormal = n;
m_iscollidingGround = true;
@@ -1403,7 +1377,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// if error is zero, use position control; otherwise, velocity control
if (MathF.Abs(fz) < 0.01f)
{
ctz.Z = 0;
ctv.Z = 0;
}
else
{
@@ -1416,16 +1390,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
else if (tmp < 0.1f)
fz = 0.1f * MathF.Sign(fz);
ctz.Z = fz;
ctv.Z = fz;
}
}
}
//******************************************
if (!m_iscolliding)
m_collideNormal.Z = 0;
CollideNormal.Z = 0;
bool tviszero = ctz.IsZero();
if (!tviszero)
{
@@ -1433,15 +1407,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// movement relative to surface if moving on it
// dont disturbe vertical movement, ie jumps
if (m_iscolliding && !m_flying && ctz.Z == 0f && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f)
if (m_iscolliding && !m_flying && ctv.Z == 0f && CollideNormal.Z > 0.2f && CollideNormal.Z < 0.94f)
{
float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y;
ctz.X *= MathF.Sqrt(1 - m_collideNormal.X * m_collideNormal.X);
ctz.Y *= MathF.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y);
ctz.Z -= p;
if (ctz.Z < 0f)
ctz.Z *= 2f;
float p = ctv.X * CollideNormal.X + ctv.Y * CollideNormal.Y;
ctv.X *= MathF.Sqrt(1 - CollideNormal.X * CollideNormal.X);
ctv.Y *= MathF.Sqrt(1 - CollideNormal.Y * CollideNormal.Y);
ctv.Z -= p;
if (ctv.Z < 0f)
ctv.Z *= 2f;
}
}
@@ -1458,6 +1431,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
_zeroFlag = true;
_zeroPosition = _position;
if(!m_pidControllerActive)
{
float pidd0833 = PID_D * 0.833f;
vec.X -= vel.X * pidd0833;
vec.Y -= vel.Y * pidd0833;
}
}
if (m_pidControllerActive)
{
@@ -1480,12 +1459,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
_zeroFlag = false;
float pidd0833 = PID_D * 0.833f;
vec.X += (ctz.X - vel.X) * pidd0833;
vec.Y += (ctz.Y - vel.Y) * pidd0833;
vec.X += (ctv.X - vel.X) * pidd0833;
vec.Y += (ctv.Y - vel.Y) * pidd0833;
// hack for breaking on fall
if (ctz.Z == -9999f)
if (ctv.Z == -9999f)
vec.Z += -vel.Z * PID_D - m_scenegravityForceZ;
;
}
}
else
@@ -1498,36 +1476,36 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (!m_flying)
{
// we are on a surface
if (ctz.Z > 0f)
if (ctv.Z > 0f)
{
// moving up or JUMPING
vec.Z += (ctz.Z - vel.Z) * PID_D * 2f;
vec.X += (ctz.X - vel.X) * PID_D;
vec.Y += (ctz.Y - vel.Y) * PID_D;
vec.Z += (ctv.Z - vel.Z) * PID_D * 2f;
vec.X += (ctv.X - vel.X) * PID_D;
vec.Y += (ctv.Y - vel.Y) * PID_D;
}
else
{
// we are moving down on a surface
if (ctz.Z == 0)
if (ctv.Z == 0)
{
if (vel.Z > 0)
vec.Z -= vel.Z * PID_D * 2f;
vec.X += (ctz.X - vel.X) * PID_D;
vec.Y += (ctz.Y - vel.Y) * PID_D;
vec.X += (ctv.X - vel.X) * PID_D;
vec.Y += (ctv.Y - vel.Y) * PID_D;
}
// intencionally going down
else
{
if (ctz.Z < vel.Z)
vec.Z += (ctz.Z - vel.Z) * PID_D;
if (ctv.Z < vel.Z)
vec.Z += (ctv.Z - vel.Z) * PID_D;
else
{
}
if (MathF.Abs(ctz.X) > MathF.Abs(vel.X))
vec.X += (ctz.X - vel.X) * PID_D;
if (MathF.Abs(ctz.Y) > MathF.Abs(vel.Y))
vec.Y += (ctz.Y - vel.Y) * PID_D;
if (MathF.Abs(ctv.X) > MathF.Abs(vel.X))
vec.X += (ctv.X - vel.X) * PID_D;
if (MathF.Abs(ctv.Y) > MathF.Abs(vel.Y))
vec.Y += (ctv.Y - vel.Y) * PID_D;
}
}
@@ -1537,9 +1515,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
// We're flying and colliding with something
float pidd00625 = PID_D * 0.0625f;
vec.X += (ctz.X - vel.X) * pidd00625;
vec.Y += (ctz.Y - vel.Y) * pidd00625;
vec.Z += (ctz.Z - vel.Z) * pidd00625;
vec.X += (ctv.X - vel.X) * pidd00625;
vec.Y += (ctv.Y - vel.Y) * pidd00625;
vec.Z += (ctv.Z - vel.Z) * pidd00625;
}
}
else // ie not colliding
@@ -1547,9 +1525,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (m_flying || hoverPIDActive) //(!m_iscolliding && flying)
{
// we're in mid air suspended
vec.X += (ctz.X - vel.X) * PID_D;
vec.Y += (ctz.Y - vel.Y) * PID_D;
vec.Z += (ctz.Z - vel.Z) * PID_D;
vec.X += (ctv.X - vel.X) * PID_D;
vec.Y += (ctv.Y - vel.Y) * PID_D;
vec.Z += (ctv.Z - vel.Z) * PID_D;
}
else
@@ -1559,10 +1537,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
// d.Vector3 pos = d.BodyGetPosition(Body);
float pidd0833 = PID_D * 0.833f;
vec.X += (ctz.X - vel.X) * pidd0833;
vec.Y += (ctz.Y - vel.Y) * pidd0833;
vec.X += (ctv.X - vel.X) * pidd0833;
vec.Y += (ctv.Y - vel.Y) * pidd0833;
// hack for breaking on fall
if (ctz.Z == -9999f)
if (ctv.Z == -9999f)
vec.Z += -vel.Z * PID_D - m_scenegravityForceZ;
}
}
@@ -1890,7 +1868,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private void changeAvatarSize(strAvatarSize st)
{
m_feetOffset = st.offset;
changeSize(st.size);
}
@@ -1902,45 +1879,42 @@ namespace OpenSim.Region.PhysicsModule.ubOde
if (pSize.Z != m_size.Z)
{
float oldsz = m_size.Z;
m_size = pSize;
m_size = pSize * 0.5f;
float sz = m_size.Z;
float bot = -sz * 0.5f + m_feetOffset;
float bot = -sz;
boneOff = bot + 0.3f;
float feetsz = sz * 0.45f;
float feetsz = sz * 0.9f;
if (feetsz > 0.6f)
feetsz = 0.6f;
feetOff = bot + feetsz;
float sx = m_size.X;
AvaAvaSizeXsq = 0.4f * sx;
AvaAvaSizeXsq *= AvaAvaSizeXsq;
AvaSizeXsq = sx;
AvaSizeXsq *= AvaSizeXsq;
float sy = m_size.Y;
AvaAvaSizeYsq = 0.5f * sy;
AvaAvaSizeYsq *= AvaAvaSizeYsq;
AvaSizeYsq = sy;
AvaSizeYsq *= AvaSizeYsq;
CapsuleRadius = sx;
if (sy > CapsuleRadius)
CapsuleRadius = sy;
CapsuleRadius = MathF.Max(sx, sy);
float l = sz - CapsuleRadius;
CapsuleRadius *= 0.5f;
CapsuleSizeZ= 0.5f * sz;
//SafeNativeMethods.GeomCapsuleSetParams(capsule, r, l);
SafeNativeMethods.GeomCapsuleSetParams(collider, CapsuleRadius, l);
CapsuleSizeZ= sz;
m_mass = m_density * sx * sy * sz; // update mass
SafeNativeMethods.GeomCapsuleSetParams(collider, CapsuleRadius, 2f * l);
m_mass = 8f * m_density * sx * sy * sz; // update mass
m_massInvTimeScaled = m_mass * m_sceneInverseTimeStep;
PID_D = basePID_D * m_massInvTimeScaled;
PID_P = basePID_P * m_massInvTimeScaled;
SafeNativeMethods.MassSetBoxTotal(out ShellMass, m_mass, sx, sy, sz);
SafeNativeMethods.MassSetBoxTotal(out ShellMass, m_mass, 2f * sx, 2f * sy, 2f * sz);
SafeNativeMethods.BodySetMass(Body, ref ShellMass);
m_scenegravityForceZ = m_sceneGravityZ * m_mass;
_position.Z += (sz - oldsz) * 0.5f;
_position.Z += sz - oldsz;
SafeNativeMethods.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
UpdateAABB2D();
@@ -1980,31 +1954,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
{
if (m_orientation.NotEqual(newOri))
{
m_orientation = newOri; // keep a copy for core use
// but only use rotations around Z
m_orientation = newOri;
Orientation2D.X = newOri.Z;
Orientation2D.Y = newOri.W;
Orientation2D.Normalize();
m_orientation2D.W = newOri.W;
m_orientation2D.Z = newOri.Z;
float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
if (t > 0)
{
t = 1.0f / MathF.Sqrt(t);
m_orientation2D.W *= t;
m_orientation2D.Z *= t;
}
else
{
m_orientation2D.W = 1.0f;
m_orientation2D.Z = 0f;
}
m_orientation2D.Y = 0f;
m_orientation2D.X = 0f;
m_NativeOrientation2D.X = m_orientation2D.X;
m_NativeOrientation2D.Y = m_orientation2D.Y;
m_NativeOrientation2D.Z = m_orientation2D.Z;
m_NativeOrientation2D.W = m_orientation2D.W;
m_NativeOrientation2D.X = 0;
m_NativeOrientation2D.Y = 0;
m_NativeOrientation2D.Z = Orientation2D.X;
m_NativeOrientation2D.W = Orientation2D.Y;
if (Body != IntPtr.Zero)
{
@@ -2313,7 +2272,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
private struct strAvatarSize
{
public Vector3 size;
public float offset;
}
}
}

228
OpenSim/Region/PhysicsModules/ubOde/ODECollision.cs Normal file
View File

@@ -0,0 +1,228 @@
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 OpenSim.Region.PhysicsModules.SharedBase;
namespace OpenSim.Region.PhysicsModule.ubOde
{
public partial class ODEScene
{
internal bool CollideSpheres(Vector3 p1, float r1, Vector3 p2, float r2,
ref SafeNativeMethods.ContactGeom c)
{
float rsum = r1 + r2;
Vector3 delta = p1 - p2;
float d = delta.LengthSquared();
if (d >= rsum * rsum)
return false;
if (d < 1e-6f)
{
c.pos = Unsafe.As<Vector3, SafeNativeMethods.Vector3>(ref p1);
c.normal.X = 1;
c.normal.Y = 0;
c.normal.Z = 0;
c.depth = rsum;
return true;
}
d = MathF.Sqrt(d);
c.depth = rsum - d;
float k = r1 - 0.5f * c.depth;
delta *= 1.0f / d;
c.normal = Unsafe.As<Vector3, SafeNativeMethods.Vector3>(ref delta);
delta *= k;
delta = p1 - delta;
c.pos = Unsafe.As<Vector3, SafeNativeMethods.Vector3>(ref delta);
return true;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
internal static bool CollideVerticalChars(OdeCharacter p1, OdeCharacter p2, ref ContactPoint cp, ref int feet)
{
Vector3 tt;
tt.Z = p1._position.Z - p2._position.Z;
float absdz = Math.Abs(tt.Z);
float r = p1.CapsuleSizeZ + p2.CapsuleSizeZ;
if (absdz >= r)
return false;
tt.X = p1._position.X - p2._position.X;
tt.Y = p1._position.Y - p2._position.Y;
Vector3 v;
float r1, r2, rhsum, d;
float rsum = p1.CapsuleRadius + p2.CapsuleRadius;
r -= rsum;
if (absdz < r)
{
cp.Position = p1._position;
cp.Position.Z -= 0.5f * tt.Z;
cp.SurfaceNormal.Z = 0;
float hd = tt.X * tt.X + tt.Y * tt.Y;
if (hd < 1e-6f)
{
cp.SurfaceNormal.X = 1;
cp.SurfaceNormal.Y = 0;
cp.PenetrationDepth = rsum;
return true;
}
hd = MathF.Sqrt(hd);
d = hd;
hd = 1.0f / hd;
tt.X *= hd;
tt.Y *= hd;
v = Vector3.InverseRotateByShortQZ(tt, p1.Orientation2D);
r1 = 1.0f / MathF.Sqrt(v.X * v.X / p1.AvaSizeXsq + v.Y * v.Y / p1.AvaSizeYsq);
v = Vector3.InverseRotateByShortQZ(tt, p2.Orientation2D);
r2 = 1.0f / MathF.Sqrt(v.X * v.X / p2.AvaSizeXsq + v.Y * v.Y / p2.AvaSizeYsq);
rhsum = r1 + r2 - d;
if (rhsum < 0.5e-3f)
return false;
cp.SurfaceNormal.X = tt.X;
cp.SurfaceNormal.Y = tt.Y;
float hk = r1 - 0.5f * rhsum;
if (hk <= 0)
cp.PenetrationDepth = r1;
else
{
cp.PenetrationDepth = rhsum;
cp.Position.X -= hk * tt.X;
cp.Position.Y -= hk * tt.Y;
}
return true;
}
if (tt.Z > 0)
{
tt.Z -= r;
feet = 1;
}
else
{
tt.Z += r;
feet = -1;
}
d = tt.LengthSquared();
if (d < 1e-6f)
{
cp.Position = p1._position;
cp.SurfaceNormal.X = 0;
cp.SurfaceNormal.Y = 0;
cp.SurfaceNormal.Z = tt.Z > 0 ? 1.0f : -1.0f;
cp.PenetrationDepth = rsum;
return true;
}
d = MathF.Sqrt(d);
float k = rsum - d;
if (k < 0.5e-3f)
return false;
tt *= 1.0f / d;
v = Vector3.InverseRotateByShortQZ(tt, p1.Orientation2D);
r1 = v.X * v.X / p1.AvaSizeXsq + v.Y * v.Y / p1.AvaSizeYsq;
v = Vector3.InverseRotateByShortQZ(tt, p2.Orientation2D);
r2 = v.X * v.X / p2.AvaSizeXsq + v.Y * v.Y / p2.AvaSizeYsq;
if (d > 1.0f / MathF.Sqrt(r1) + 1.0f / MathF.Sqrt(r2))
return false;
cp.SurfaceNormal = tt;
cp.PenetrationDepth = k;
k = p1.CapsuleRadius - 0.5f * k;
tt *= k;
cp.Position = p1._position - tt;
return true;
}
internal ContactPoint SharedChrContact = new();
[MethodImpl(MethodImplOptions.AggressiveInlining)]
internal void CollideCharChar(OdeCharacter p1, OdeCharacter p2)
{
if (ContactJointCount >= maxContactJoints)
return;
int feetcollision = 0;
if (!CollideVerticalChars(p1, p2, ref SharedChrContact, ref feetcollision))
return;
p1.CollidingObj = true;
p2.CollidingObj = true;
if (feetcollision > 0)
{
p1.CollideNormal = SharedChrContact.SurfaceNormal;
p1.IsColliding = true;
}
else if (feetcollision < 0)
{
p2.CollideNormal = -SharedChrContact.SurfaceNormal;
p2.IsColliding = true;
SharedChrContact.CharacterFeet = true;
}
contactSharedForJoints.surface.mu = 0;
contactSharedForJoints.surface.bounce = 0;
contactSharedForJoints.geom.pos = Unsafe.As<Vector3, SafeNativeMethods.Vector3>(ref SharedChrContact.Position);
contactSharedForJoints.geom.normal = Unsafe.As<Vector3, SafeNativeMethods.Vector3>(ref SharedChrContact.SurfaceNormal);
contactSharedForJoints.geom.depth = SharedChrContact.PenetrationDepth;
IntPtr Joint = CreateCharContacJoint();
if (Joint == IntPtr.Zero)
return;
SafeNativeMethods.JointAttach(Joint, p1.Body, p2.Body);
if (p1.CollisionScore < float.MaxValue)
p1.CollisionScore += 1.0f;
if (p2.CollisionScore < float.MaxValue)
p2.CollisionScore += 1.0f;
bool p1events = p1.SubscribedEvents();
bool p2events = p2.SubscribedEvents();
if (!p2events && !p1events)
return;
Vector3 vel = Vector3.Zero;
if (p2.IsPhysical)
vel = p2.rootVelocity;
if (p1.IsPhysical)
vel -= p1.rootVelocity;
SharedChrContact.RelativeSpeed = Vector3.Dot(vel, SharedChrContact.SurfaceNormal);
if (p2events)
{
p2.AddCollisionEvent(p1.ParentActor.m_baseLocalID, SharedChrContact);
}
if (p1events)
{
SharedChrContact.SurfaceNormal = -SharedChrContact.SurfaceNormal;
SharedChrContact.RelativeSpeed = -SharedChrContact.RelativeSpeed;
SharedChrContact.CharacterFeet = feetcollision > 0;
p1.AddCollisionEvent(p2.ParentActor.m_baseLocalID, SharedChrContact);
}
}
}
}

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

@@ -164,7 +164,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
public Object arg;
}
public class ODEScene : PhysicsScene
public partial class ODEScene : PhysicsScene
{
private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
@@ -551,6 +551,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
return SafeNativeMethods.JointCreateContactPtr(world, JointContactGroup, contact);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
private IntPtr CreateCharContacJoint()
{
ContactJointCount++;
IntPtr contact = new(GlobalContactsArray.ToInt64() + (Int64)(ContactJointCount * SafeNativeMethods.SizeOfContact));
Marshal.StructureToPtr(contactSharedForJoints, contact, false);
return SafeNativeMethods.JointCreateContactPtr(world, JointContactGroup, contact);
}
SafeNativeMethods.ContactGeom altWorkContact = new();
/// <summary>
@@ -994,90 +1004,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
}
}
[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.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;
IntPtr Joint;
bool FeetCollision = false;
int ncontacts = 0;
ContactPoint maccountContact = new();
float minDepth = float.MaxValue;
float maxDepth = float.MinValue;
contactSharedForJoints.surface.mu = 0;
contactSharedForJoints.surface.bounce = 0;
bool useAltcontact;
for (int i = 0; i < count; ++i)
{
ref SafeNativeMethods.ContactGeom curctc = ref m_contacts[i];
useAltcontact = false;
if (p1.Collide(p2.collider, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision))
{
p1.CollidingObj = true;
p1.CollidingObj = true;
Joint = useAltcontact ?
CreateContacJoint(ref altWorkContact, false) :
CreateContacJoint(ref curctc, false);
if (Joint == IntPtr.Zero)
break;
SafeNativeMethods.JointAttach(Joint, p1.Body, p2.Body);
ncontacts++;
if (curctc.depth > maxDepth)
{
maxDepth = curctc.depth;
maccountContact.PenetrationDepth = maxDepth;
maccountContact.Position = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curctc.pos);
maccountContact.CharacterFeet = FeetCollision;
}
if (curctc.depth < minDepth)
{
minDepth = curctc.depth;
maccountContact.SurfaceNormal = Unsafe.As<SafeNativeMethods.Vector3, Vector3>(ref curctc.normal);
}
}
}
if (ncontacts > 0)
{
Collision_accounting_events(p1, p2, ref maccountContact);
}
}
private static void Collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ref ContactPoint contact)
{
@@ -1188,11 +1114,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
chr.IsColliding = false;
chr.CollidingObj = false;
if (chr.Body != IntPtr.Zero && chr.collider != IntPtr.Zero)
{
SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, CharPrimNearCallback);
SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, CharPrimNearCallback);
}
SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, CharPrimNearCallback);
SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, CharPrimNearCallback);
}
}
else
@@ -1222,27 +1145,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
OdeCharacter chr2 = charsSpan[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);
}
}