diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs index e3b8efcab3..ad904103f2 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs @@ -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(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(ref contact.normal); + CollideNormal = Unsafe.As(ref contact.normal); IsColliding = true; } } @@ -1128,7 +1100,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde feetcollision = true; if (h < boneOff) { - m_collideNormal = Unsafe.As(ref contact.normal); + CollideNormal = Unsafe.As(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; } } } diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECollision.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECollision.cs new file mode 100644 index 0000000000..2be2cf563a --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODECollision.cs @@ -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(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(ref delta); + + delta *= k; + delta = p1 - delta; + c.pos = Unsafe.As(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(ref SharedChrContact.Position); + contactSharedForJoints.geom.normal = Unsafe.As(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); + } + } + } +} + diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs index 975bbeca31..fe975f5867 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs @@ -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(); /// @@ -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(ref curctc.pos); - maccountContact.CharacterFeet = FeetCollision; - } - - if (curctc.depth < minDepth) - { - minDepth = curctc.depth; - maccountContact.SurfaceNormal = Unsafe.As(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); } }