diff options
Diffstat (limited to 'OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs | 48 |
1 files changed, 21 insertions, 27 deletions
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs index ce10065..23b1192 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs | |||
@@ -41,13 +41,7 @@ | |||
41 | // Extensive change Ubit 2012 | 41 | // Extensive change Ubit 2012 |
42 | 42 | ||
43 | using System; | 43 | using System; |
44 | using System.Collections.Generic; | ||
45 | using System.Reflection; | ||
46 | using System.Runtime.InteropServices; | ||
47 | using log4net; | ||
48 | using OpenMetaverse; | 44 | using OpenMetaverse; |
49 | using OdeAPI; | ||
50 | using OpenSim.Framework; | ||
51 | using OpenSim.Region.PhysicsModules.SharedBase; | 45 | using OpenSim.Region.PhysicsModules.SharedBase; |
52 | 46 | ||
53 | namespace OpenSim.Region.PhysicsModule.ubOde | 47 | namespace OpenSim.Region.PhysicsModule.ubOde |
@@ -342,9 +336,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
342 | m_amEfect = 1.0f ; // turn it on | 336 | m_amEfect = 1.0f ; // turn it on |
343 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | 337 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; |
344 | 338 | ||
345 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | 339 | if (rootPrim.Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(rootPrim.Body) |
346 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | 340 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) |
347 | d.BodyEnable(rootPrim.Body); | 341 | SafeNativeMethods.BodyEnable(rootPrim.Body); |
348 | 342 | ||
349 | break; | 343 | break; |
350 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | 344 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
@@ -361,9 +355,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
361 | m_lmEfect = 1.0f; // turn it on | 355 | m_lmEfect = 1.0f; // turn it on |
362 | 356 | ||
363 | m_ffactor = 0.0f; | 357 | m_ffactor = 0.0f; |
364 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | 358 | if (rootPrim.Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(rootPrim.Body) |
365 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | 359 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) |
366 | d.BodyEnable(rootPrim.Body); | 360 | SafeNativeMethods.BodyEnable(rootPrim.Body); |
367 | break; | 361 | break; |
368 | case Vehicle.LINEAR_MOTOR_OFFSET: | 362 | case Vehicle.LINEAR_MOTOR_OFFSET: |
369 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | 363 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); |
@@ -399,9 +393,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
399 | m_amEfect = 1.0f; // turn it on | 393 | m_amEfect = 1.0f; // turn it on |
400 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | 394 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; |
401 | 395 | ||
402 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | 396 | if (rootPrim.Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(rootPrim.Body) |
403 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | 397 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) |
404 | d.BodyEnable(rootPrim.Body); | 398 | SafeNativeMethods.BodyEnable(rootPrim.Body); |
405 | break; | 399 | break; |
406 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | 400 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
407 | if (pValue.X < m_timestep) pValue.X = m_timestep; | 401 | if (pValue.X < m_timestep) pValue.X = m_timestep; |
@@ -419,9 +413,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
419 | m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; | 413 | m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; |
420 | 414 | ||
421 | m_ffactor = 0.0f; | 415 | m_ffactor = 0.0f; |
422 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | 416 | if (rootPrim.Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(rootPrim.Body) |
423 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | 417 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) |
424 | d.BodyEnable(rootPrim.Body); | 418 | SafeNativeMethods.BodyEnable(rootPrim.Body); |
425 | break; | 419 | break; |
426 | case Vehicle.LINEAR_MOTOR_OFFSET: | 420 | case Vehicle.LINEAR_MOTOR_OFFSET: |
427 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | 421 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); |
@@ -772,30 +766,30 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
772 | { | 766 | { |
773 | IntPtr Body = rootPrim.Body; | 767 | IntPtr Body = rootPrim.Body; |
774 | 768 | ||
775 | d.Mass dmass; | 769 | SafeNativeMethods.Mass dmass; |
776 | d.BodyGetMass(Body, out dmass); | 770 | SafeNativeMethods.BodyGetMass(Body, out dmass); |
777 | 771 | ||
778 | d.Quaternion rot = d.BodyGetQuaternion(Body); | 772 | SafeNativeMethods.Quaternion rot = SafeNativeMethods.BodyGetQuaternion(Body); |
779 | Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object | 773 | Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object |
780 | Quaternion rotq = objrotq; // rotq = rotation of object | 774 | Quaternion rotq = objrotq; // rotq = rotation of object |
781 | rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame | 775 | rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame |
782 | Quaternion irotq = Quaternion.Inverse(rotq); | 776 | Quaternion irotq = Quaternion.Inverse(rotq); |
783 | 777 | ||
784 | d.Vector3 dvtmp; | 778 | SafeNativeMethods.Vector3 dvtmp; |
785 | Vector3 tmpV; | 779 | Vector3 tmpV; |
786 | Vector3 curVel; // velocity in world | 780 | Vector3 curVel; // velocity in world |
787 | Vector3 curAngVel; // angular velocity in world | 781 | Vector3 curAngVel; // angular velocity in world |
788 | Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame | 782 | Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame |
789 | Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame | 783 | Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame |
790 | d.Vector3 dtorque = new d.Vector3(); | 784 | SafeNativeMethods.Vector3 dtorque = new SafeNativeMethods.Vector3(); |
791 | 785 | ||
792 | dvtmp = d.BodyGetLinearVel(Body); | 786 | dvtmp = SafeNativeMethods.BodyGetLinearVel(Body); |
793 | curVel.X = dvtmp.X; | 787 | curVel.X = dvtmp.X; |
794 | curVel.Y = dvtmp.Y; | 788 | curVel.Y = dvtmp.Y; |
795 | curVel.Z = dvtmp.Z; | 789 | curVel.Z = dvtmp.Z; |
796 | Vector3 curLocalVel = curVel * irotq; // current velocity in local | 790 | Vector3 curLocalVel = curVel * irotq; // current velocity in local |
797 | 791 | ||
798 | dvtmp = d.BodyGetAngularVel(Body); | 792 | dvtmp = SafeNativeMethods.BodyGetAngularVel(Body); |
799 | curAngVel.X = dvtmp.X; | 793 | curAngVel.X = dvtmp.X; |
800 | curAngVel.Y = dvtmp.Y; | 794 | curAngVel.Y = dvtmp.Y; |
801 | curAngVel.Z = dvtmp.Z; | 795 | curAngVel.Z = dvtmp.Z; |
@@ -839,7 +833,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
839 | { | 833 | { |
840 | // have offset, do it now | 834 | // have offset, do it now |
841 | tmpV *= dmass.mass; | 835 | tmpV *= dmass.mass; |
842 | d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); | 836 | SafeNativeMethods.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); |
843 | } | 837 | } |
844 | else | 838 | else |
845 | { | 839 | { |
@@ -862,7 +856,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
862 | if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) | 856 | if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) |
863 | { | 857 | { |
864 | // d.Vector3 pos = d.BodyGetPosition(Body); | 858 | // d.Vector3 pos = d.BodyGetPosition(Body); |
865 | d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom); | 859 | SafeNativeMethods.Vector3 pos = SafeNativeMethods.GeomGetPosition(rootPrim.prim_geom); |
866 | pos.Z -= 0.21f; // minor offset that seems to be always there in sl | 860 | pos.Z -= 0.21f; // minor offset that seems to be always there in sl |
867 | 861 | ||
868 | float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); | 862 | float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); |
@@ -1181,7 +1175,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
1181 | 1175 | ||
1182 | if (force.X != 0 || force.Y != 0 || force.Z != 0) | 1176 | if (force.X != 0 || force.Y != 0 || force.Z != 0) |
1183 | { | 1177 | { |
1184 | d.BodyAddForce(Body, force.X, force.Y, force.Z); | 1178 | SafeNativeMethods.BodyAddForce(Body, force.X, force.Y, force.Z); |
1185 | } | 1179 | } |
1186 | 1180 | ||
1187 | if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) | 1181 | if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) |
@@ -1191,15 +1185,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde | |||
1191 | dtorque.Y = torque.Y; | 1185 | dtorque.Y = torque.Y; |
1192 | dtorque.Z = torque.Z; | 1186 | dtorque.Z = torque.Z; |
1193 | 1187 | ||
1194 | d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); | 1188 | SafeNativeMethods.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); |
1195 | d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame | 1189 | SafeNativeMethods.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame |
1196 | } | 1190 | } |
1197 | 1191 | ||
1198 | torque = rootPrim.m_torque; | 1192 | torque = rootPrim.m_torque; |
1199 | torque += rootPrim.m_angularForceacc; | 1193 | torque += rootPrim.m_angularForceacc; |
1200 | rootPrim.m_angularForceacc = Vector3.Zero; | 1194 | rootPrim.m_angularForceacc = Vector3.Zero; |
1201 | if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) | 1195 | if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) |
1202 | d.BodyAddTorque(Body,torque.X, torque.Y, torque.Z); | 1196 | SafeNativeMethods.BodyAddTorque(Body,torque.X, torque.Y, torque.Z); |
1203 | } | 1197 | } |
1204 | } | 1198 | } |
1205 | } | 1199 | } |