diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs index ac007aef2e..02b9c9a3af 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs @@ -62,11 +62,12 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public bool MouseLook; } + [StructLayout(LayoutKind.Sequential, Pack = 16)] public struct AABB2D { public float minx; - public float miny; public float maxx; + public float miny; public float maxy; } diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs index 66459c10b2..2f3a5c6d48 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs @@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ using System; +using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Security; using OMV = OpenMetaverse; @@ -516,9 +517,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde } [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodyDisable(IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodyEnable(IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] @@ -541,10 +544,30 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity] internal extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Vector3 BodyGetAngularVel(IntPtr body) { unsafe { return *(BodyGetAngularVelUnsafe(body)); } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal static OMV.Vector3 BodyGetAngularVelOMV(IntPtr body) + { + unsafe + { + Vector3* vtmp = BodyGetAngularVelUnsafe(body); + return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z); + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal static OMV.Vector3 BodyGetAngularVelOMVforAvatar(IntPtr body) + { + unsafe + { + Vector3* vtmp = BodyGetAngularVelUnsafe(body); + return new OMV.Vector3(0, 0, (float)Math.Round(vtmp->Z, 3)); + } + } [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr BodyGetData(IntPtr body); @@ -573,10 +596,20 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity] internal extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Vector3 BodyGetLinearVel(IntPtr body) { unsafe { return *(BodyGetLinearVelUnsafe(body)); } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal static OMV.Vector3 BodyGetLinearVelOMV(IntPtr body) + { + unsafe + { + Vector3* vtmp = BodyGetLinearVelUnsafe(body); + return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z); + } + } [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity] internal static extern void BodyGetMass(IntPtr body, out Mass mass); @@ -589,20 +622,40 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity] internal extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Vector3 BodyGetPosition(IntPtr body) { unsafe { return *(BodyGetPositionUnsafe(body)); } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal static OMV.Vector3 BodyGetPositionOMV(IntPtr body) + { + unsafe + { + Vector3* vtmp = BodyGetPositionUnsafe(body); + return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z); + } + } [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity] internal static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity] internal extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Quaternion BodyGetQuaternion(IntPtr body) { unsafe { return *(BodyGetQuaternionUnsafe(body)); } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + internal static OMV.Quaternion BodyGetQuaternionOMV(IntPtr body) + { + unsafe + { + Quaternion* q = BodyGetQuaternionUnsafe(body); + return new OMV.Quaternion(q->X, q->Y, q->Z, q->W); + } + } [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity] internal static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); @@ -634,69 +687,91 @@ namespace OpenSim.Region.PhysicsModule.ubOde internal static extern IntPtr dBodyGetNextGeom(IntPtr Geom); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern bool BodyIsEnabled(IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAutoDisableDefaults(IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAutoDisableSteps(IntPtr body, int steps); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAutoDisableTime(IntPtr body, dReal time); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetData(IntPtr body, IntPtr data); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetFiniteRotationMode(IntPtr body, int mode); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetLinearDamping(IntPtr body, dReal scale); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAngularDamping(IntPtr body, dReal scale); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern dReal BodyGetLinearDamping(IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern dReal BodyGetAngularDamping(IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern dReal BodyGetLinearDampingThreshold(IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern dReal BodyGetAngularDampingThreshold(IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetGravityMode(IntPtr body, bool mode); /// @@ -708,33 +783,43 @@ namespace OpenSim.Region.PhysicsModule.ubOde internal static extern void dBodySetGyroscopicMode(IntPtr body, int enabled); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetMass(IntPtr body, ref Mass mass); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetQuaternion(IntPtr body, ref Quaternion q); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetQuaternion(IntPtr body, ref dReal w); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetRotation(IntPtr body, ref Matrix3 R); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetRotation(IntPtr body, ref dReal M00); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity] @@ -761,6 +846,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde internal static extern void CloseODE(); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity] @@ -768,6 +854,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz) { NTotalGeoms++; @@ -776,6 +863,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length) { NTotalGeoms++; @@ -808,6 +896,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateOSTerrain"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr CreateiOSTerrain(IntPtr space, IntPtr data, int bPlaceable); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static IntPtr CreateOSTerrain(IntPtr space, IntPtr data, int bPlaceable) { NTotalGeoms++; @@ -830,6 +919,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d) { NTotalGeoms++; @@ -838,6 +928,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr CreateiRay(IntPtr space, dReal length); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static IntPtr CreateRay(IntPtr space, dReal length) { NTotalGeoms++; @@ -846,6 +937,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr CreateiSphere(IntPtr space, dReal radius); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static IntPtr CreateSphere(IntPtr space, dReal radius) { NTotalGeoms++; @@ -855,6 +947,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static IntPtr CreateTriMesh(IntPtr space, IntPtr data, TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback) { diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs index 66f8ef5845..bebbb52cbe 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs @@ -237,7 +237,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde Name = avName; - UpdateShortAABB(); + UpdateAABB2D(); AddChange(changes.Add, null); } @@ -1222,71 +1222,65 @@ namespace OpenSim.Region.PhysicsModule.ubOde // so force it back to identity SafeNativeMethods.BodySetQuaternion(Body, ref m_NativeOrientation2D); - SafeNativeMethods.Vector3 dtmp = SafeNativeMethods.BodyGetPosition(Body); - Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - + _position = SafeNativeMethods.BodyGetPositionOMV(Body); // check outbounds forcing to be in world bool fixbody = false; - float tmp = localpos.X; - if ((Single.IsNaN(tmp) || Single.IsInfinity(tmp))) + if ((Single.IsNaN(_position.X) || Single.IsInfinity(_position.X))) { fixbody = true; - localpos.X = 128f; + _position.X = 128f; } - else if (tmp < 0.0f) + else if (_position.X < 0.0f) { fixbody = true; - localpos.X = 0.1f; + _position.X = 0.1f; } - else if (tmp > m_parent_scene.WorldExtents.X - 0.1f) + else if (_position.X > m_parent_scene.WorldExtents.X - 0.1f) { fixbody = true; - localpos.X = m_parent_scene.WorldExtents.X - 0.1f; + _position.X = m_parent_scene.WorldExtents.X - 0.1f; } - tmp = localpos.Y; - if ((Single.IsNaN(tmp) || Single.IsInfinity(tmp))) + if ((Single.IsNaN(_position.Y) || Single.IsInfinity(_position.Y))) { fixbody = true; - localpos.X = 128f; + _position.Y = 128f; } - else if (tmp < 0.0f) + else if (_position.Y < 0.0f) { fixbody = true; - localpos.Y = 0.1f; + _position.Y = 0.1f; } - else if (tmp > m_parent_scene.WorldExtents.Y - 0.1) + else if (_position.Y > m_parent_scene.WorldExtents.Y - 0.1f) { fixbody = true; - localpos.Y = m_parent_scene.WorldExtents.Y - 0.1f; + _position.Y = m_parent_scene.WorldExtents.Y - 0.1f; } - tmp = localpos.Z; - if ((Single.IsNaN(tmp) || Single.IsInfinity(tmp))) + if ((Single.IsNaN(_position.Z) || Single.IsInfinity(_position.Z))) { fixbody = true; - localpos.Z = 128f; - } - - if (m_pidControllerActive == false) - { - _zeroPosition = localpos; + _position.Z = 128f; } if (fixbody) { m_freemove = false; - SafeNativeMethods.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); + SafeNativeMethods.BodySetPosition(Body, _position.X, _position.Y, _position.Z); } - float breakfactor; + if (m_pidControllerActive == false) + { + _zeroPosition = _position; + } - Vector3 vec = Vector3.Zero; - dtmp = SafeNativeMethods.BodyGetLinearVel(Body); - Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + //Update AABB + UpdateAABB2D(); + + float aabbminz = _position.Z - CapsuleSizeZ; + //float aabbmaxz = _position.Z + CapsuleSizeZ; Vector3 ctz = m_targetVelocity; - if (m_alwaysRun) { ctz.X *= m_runMultiplier; @@ -1298,22 +1292,21 @@ namespace OpenSim.Region.PhysicsModule.ubOde ctz.Y *= m_walkMultiplier; } + Vector3 vel = SafeNativeMethods.BodyGetLinearVelOMV(Body); + //****************************************** // colide with land - - //SafeNativeMethods.GeomGetAABB(capsule, out SafeNativeMethods.AABB aabb); - SafeNativeMethods.GeomGetAABB(collider, out SafeNativeMethods.AABB aabb); - float chrminZ = aabb.MinZ; // move up a bit - Vector3 posch = localpos; - + float tmpX = _position.X; + float tmpY = _position.Y; if (m_flying) { - posch.X += vel.X * m_sceneTimeStep; - posch.Y += vel.Y * m_sceneTimeStep; + tmpX += vel.X * m_sceneTimeStep; + tmpY += vel.Y * m_sceneTimeStep; } - float terrainheight = m_parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); - if (chrminZ < terrainheight) + Vector3 vec = Vector3.Zero; + float terrainheight = m_parent_scene.GetTerrainHeightAtXY(tmpX, tmpY); + if (aabbminz < terrainheight) { if (ctz.Z < 0) ctz.Z = 0; @@ -1324,8 +1317,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde m_haveLastFallVel = true; } - Vector3 n = m_parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y); - float depth = terrainheight - chrminZ; + Vector3 n = m_parent_scene.GetTerrainNormalAtXY(tmpX, tmpY); + float depth = terrainheight - aabbminz; vec.Z = depth * PID_P * 50; @@ -1369,8 +1362,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde ContactPoint contact = new ContactPoint(); contact.PenetrationDepth = depth; - contact.Position.X = localpos.X; - contact.Position.Y = localpos.Y; + contact.Position.X = _position.X; + contact.Position.Y = _position.Y; contact.Position.Z = terrainheight; contact.SurfaceNormal = -n; contact.RelativeSpeed = Vector3.Dot(m_lastFallVel,n); @@ -1424,9 +1417,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde } // end switch (m_PIDHoverType) // don't go underground - if (m_targetHoverHeight > terrainheight + 0.5f * (aabb.MaxZ - aabb.MinZ)) + if (m_targetHoverHeight > terrainheight + _position.Z) { - float fz = (m_targetHoverHeight - localpos.Z); + float fz = (m_targetHoverHeight - _position.Z); // if error is zero, use position control; otherwise, velocity control if (Math.Abs(fz) < 0.01f) @@ -1438,7 +1431,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde _zeroFlag = false; fz /= m_PIDHoverTau; - tmp = Math.Abs(fz); + float tmp = Math.Abs(fz); if (tmp > 50) fz = 50 * Math.Sign(fz); else if (tmp < 0.1) @@ -1473,6 +1466,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde } } + float breakfactor; if (!m_freemove) { // if velocity is zero, use position control; otherwise, velocity control @@ -1484,7 +1478,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (!_zeroFlag) { _zeroFlag = true; - _zeroPosition = localpos; + _zeroPosition = _position; } if (m_pidControllerActive) { @@ -1495,12 +1489,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde float pidd2 = PID_D * 2f; float pidp5 = PID_P * 5; - vec.X = -vel.X * pidd2 + (_zeroPosition.X - localpos.X) * pidp5; - vec.Y = -vel.Y * pidd2 + (_zeroPosition.Y - localpos.Y) * pidp5; + vec.X = -vel.X * pidd2 + (_zeroPosition.X - _position.X) * pidp5; + vec.Y = -vel.Y * pidd2 + (_zeroPosition.Y - _position.Y) * pidp5; if (vel.Z > 0) - vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - _position.Z) * PID_P; else - vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f; + vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - _position.Z) * PID_P) * 0.2f; } } else @@ -1623,9 +1617,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde //Added for auto fly height. Kitto Flora float target_altitude = terrainheight + MinimumGroundFlightOffset; - if (localpos.Z < target_altitude) + if (_position.Z < target_altitude) { - vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; + vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f; } // end add Kitto Flora } @@ -1635,15 +1629,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde vec.Z -= m_scenegravityForceZ * m_buoyancy; } - if (vec.IsFinite()) - { - if ((vec.X != 0 || vec.Y != 0 || vec.Z != 0)) - SafeNativeMethods.BodyAddForce(Body, vec.X, vec.Y, vec.Z); - } - - // update our local ideia of position velocity and aceleration - _position = localpos; - UpdateShortAABB(); + if ((vec.Z != 0 || vec.X != 0 || vec.Y != 0)) + SafeNativeMethods.BodyAddForce(Body, vec.X, vec.Y, vec.Z); if (_zeroFlag) { @@ -1658,11 +1645,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde a = (_velocity - a) * m_sceneInverseTimeStep; SetSmooth(ref _acceleration, ref a, 2); - dtmp = SafeNativeMethods.BodyGetAngularVel(Body); - m_rotationalVelocity.X = 0f; - m_rotationalVelocity.Y = 0f; - m_rotationalVelocity.Z = dtmp.Z; - Math.Round(m_rotationalVelocity.Z, 3); + m_rotationalVelocity = SafeNativeMethods.BodyGetAngularVelOMVforAvatar(Body); } } @@ -1674,14 +1657,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde v.Z = (float)Math.Round(v.Z, digits); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void SetSmooth(ref Vector3 dst, ref Vector3 value) - { - dst.X = 0.1f * dst.X + 0.9f * value.X; - dst.Y = 0.1f * dst.Y + 0.9f * value.Y; - dst.Z = 0.1f * dst.Z + 0.9f * value.Z; - } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) { @@ -1695,7 +1670,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde dst.Z = (float)Math.Round(dst.Z, rounddigits); } - /// /// Updates the reported position and velocity. /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording @@ -1703,6 +1677,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde /// copy and outbounds now done in move(..) at ode rate /// /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void UpdatePositionAndVelocity() { } @@ -1989,7 +1964,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde _position.Z += (sz - oldsz) * 0.5f; SafeNativeMethods.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - UpdateShortAABB(); + UpdateAABB2D(); m_bodydisablecontrol = 0; _zeroFlag = false; @@ -2013,8 +1988,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde SafeNativeMethods.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); SafeNativeMethods.BodyEnable(Body); } - UpdateShortAABB(); _position = newPos; + UpdateAABB2D(); + m_freemove = false; _zeroFlag = false; m_pidControllerActive = true; @@ -2057,6 +2033,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde SafeNativeMethods.BodyEnable(Body); } } + else if (Body != IntPtr.Zero) + SafeNativeMethods.BodyEnable(Body); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -2198,12 +2176,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde m_useHoverPID = active; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] private void donullchange() { } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void UpdateShortAABB() + public void UpdateAABB2D() { _AABB2D.minx = _position.X - CapsuleRadius; _AABB2D.maxx = _position.X + CapsuleRadius; diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs index 95adeec483..30469b758c 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs @@ -566,8 +566,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde SafeNativeMethods.Vector3 dtmp; if (!childPrim && Body != IntPtr.Zero) { - dtmp = SafeNativeMethods.BodyGetPosition(Body); - return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + return SafeNativeMethods.BodyGetPositionOMV(Body); } else if (m_prim_geom != IntPtr.Zero) { @@ -1507,12 +1506,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde // get current orientation to lock - SafeNativeMethods.Quaternion dcur = SafeNativeMethods.BodyGetQuaternion(Body); - Quaternion curr; // crap convertion between identical things - curr.X = dcur.X; - curr.Y = dcur.Y; - curr.Z = dcur.Z; - curr.W = dcur.W; + Quaternion curr= SafeNativeMethods.BodyGetQuaternionOMV(Body); Vector3 ax; int i = 0;