From 8428b25939d39711e732eeb3928e8a8e64aad8a9 Mon Sep 17 00:00:00 2001 From: KittoFlora Date: Mon, 26 Oct 2009 00:10:23 +0100 Subject: Add llRotLookat pt1. --- .../BasicPhysicsPlugin/BasicPhysicsActor.cs | 20 ++++++ .../BulletDotNETPlugin/BulletDotNETCharacter.cs | 6 ++ .../Physics/BulletDotNETPlugin/BulletDotNETPrim.cs | 5 ++ .../Region/Physics/BulletXPlugin/BulletXPlugin.cs | 20 ++++++ OpenSim/Region/Physics/Manager/PhysicsActor.cs | 13 +++- OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 22 ++++++ OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs | 4 +- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 83 ++++++++++++++++------ OpenSim/Region/Physics/POSPlugin/POSCharacter.cs | 21 ++++++ OpenSim/Region/Physics/POSPlugin/POSPrim.cs | 20 ++++++ OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs | 44 ++++++++++++ 11 files changed, 234 insertions(+), 24 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs index 8d8b3fe..2b905b6 100644 --- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs +++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs @@ -307,6 +307,26 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin set { return; } } + public override Quaternion APIDTarget + { + set { return; } + } + + public override bool APIDActive + { + set { return; } + } + + public override float APIDStrength + { + set { return; } + } + + public override float APIDDamping + { + set { return; } + } + public override void SubscribeEvents(int ms) { } diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs index 8da9687..d7aed3c 100644 --- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs @@ -627,6 +627,12 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin { set { return; } } + + public override Quaternion APIDTarget { set { return; } } + public override bool APIDActive { set { return; } } + public override float APIDStrength { set { return; } } + public override float APIDDamping { set { return; } } + /// /// Adds the force supplied to the Target Velocity diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs index f22ea71..977b463 100644 --- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs @@ -566,6 +566,11 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + public override Quaternion APIDTarget { set { return; } } + public override bool APIDActive { set { return; } } + public override float APIDStrength { set { return; } } + public override float APIDDamping { set { return; } } + public override void AddForce(PhysicsVector force, bool pushforce) { diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs index abed8df..14b8f39 100644 --- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs +++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs @@ -1236,6 +1236,26 @@ namespace OpenSim.Region.Physics.BulletXPlugin public override PIDHoverType PIDHoverType { set { return; } } public override float PIDHoverTau { set { return; } } + public override OpenMetaverse.Quaternion APIDTarget + { + set { return; } + } + + public override bool APIDActive + { + set { return; } + } + + public override float APIDStrength + { + set { return; } + } + + public override float APIDDamping + { + set { return; } + } + public override void SubscribeEvents(int ms) { diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 7603131..566746a 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -230,7 +230,12 @@ namespace OpenSim.Region.Physics.Manager public abstract PIDHoverType PIDHoverType { set;} public abstract float PIDHoverTau { set;} - + // For RotLookAt + public abstract Quaternion APIDTarget { set;} + public abstract bool APIDActive { set;} + public abstract float APIDStrength { set;} + public abstract float APIDDamping { set;} + public abstract void AddForce(PhysicsVector force, bool pushforce); public abstract void AddAngularForce(PhysicsVector force, bool pushforce); public abstract void SetMomentum(PhysicsVector momentum); @@ -463,6 +468,12 @@ namespace OpenSim.Region.Physics.Manager public override bool PIDHoverActive { set { return; } } public override PIDHoverType PIDHoverType { set { return; } } public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget { set { return; } } + public override bool APIDActive { set { return; } } + public override float APIDStrength { set { return; } } + public override float APIDDamping { set { return; } } + public override void SetMomentum(PhysicsVector momentum) { diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs index 71ace16..8272083 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -1205,6 +1205,28 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool PIDHoverActive { set { return; } } public override PIDHoverType PIDHoverType { set { return; } } public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget + { + set { return; } + } + + public override bool APIDActive + { + set { return; } + } + + public override float APIDStrength + { + set { return; } + } + + public override float APIDDamping + { + set { return; } + } + + public override void SubscribeEvents(int ms) { diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs index 467eba0..8b57f06 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs @@ -119,7 +119,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_VhoverEfficiency = 0f; private float m_VhoverTimescale = 0f; private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height - private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. @@ -478,7 +478,7 @@ namespace OpenSim.Region.Physics.OdePlugin Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object m_dir *= rotq; // apply obj rotation to velocity vector - // add Gravity andBuoyancy + // add Gravity and Buoyancy // KF: So far I have found no good method to combine a script-requested // .Z velocity and gravity. Therefore only 0g will used script-requested // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index f59f0ae..e1bf996 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -1,15 +1,4 @@ /* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. * @@ -34,6 +23,18 @@ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. */ using System; using System.Collections.Generic; @@ -82,6 +83,14 @@ namespace OpenSim.Region.Physics.OdePlugin private float PID_D = 35f; private float PID_G = 25f; private bool m_usePID = false; + + private Quaternion m_APIDTarget = new Quaternion(); + private float m_APIDStrength = 0.5f; + private float m_APIDDamping = 0.5f; + + private float APID_D = 35f; + private float APID_G = 25f; + private bool m_useAPID = false; // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), // and are for non-VEHICLES only. @@ -93,7 +102,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_targetHoverHeight = 0f; private float m_groundHeight = 0f; private float m_waterHeight = 0f; - private float m_buoyancy = 0f; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + private float m_buoyancy = 0f; //Set by llSetBuoyancy(), for non-vehicles. // private float m_tensor = 5f; private int body_autodisable_frames = 20; @@ -1586,21 +1595,18 @@ Console.WriteLine(" JointCreateFixed"); //m_log.Info(m_collisionFlags.ToString()); - //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? + //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up - // gravityz multiplier = 1 - m_buoyancy - fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; + // NB Prims in ODE are no subject to global gravity + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass if (m_usePID) { //Console.WriteLine("PID " + m_primName); - // KF - this is for object move? eg. llSetPos() ? + // KF - this is for object MoveToTarget. + //if (!d.BodyIsEnabled(Body)) //d.BodySetForce(Body, 0f, 0f, 0f); - // If we're using the PID controller, then we have no gravity - //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... - fz = 0f; // no lock; for now it's only called from within Simulate() @@ -1744,8 +1750,37 @@ Console.WriteLine(" JointCreateFixed"); // We're flying and colliding with something fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); } - } + } // end m_useHoverPID && !m_usePID + + if (m_useAPID) + { + // RotLookAt, apparently overrides all other rotation sources. Inputs: + // Quaternion m_APIDTarget + // float m_APIDStrength // perhaps ratio other forces to lookat force? + // float m_APIDDamping //'seconds to critically damps in'[sic] + // Factors: + // float APID_D + // float APID_G + + // get present body rotation + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; + float diff_angle; + Vector3 diff_axis; + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); + diff_axis.Normalize(); + PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); + float RLAservo = timestep / m_APIDDamping; + rotforce = rotforce * RLAservo * m_mass; + d.BodyAddTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); + + // d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); + + + } // end m_useAPID + fx *= m_mass; fy *= m_mass; //fz *= m_mass; @@ -2778,6 +2813,12 @@ Console.WriteLine(" JointCreateFixed"); } public override bool PIDActive { set { m_usePID = value; } } public override float PIDTau { set { m_PIDTau = value; } } + + // For RotLookAt + public override Quaternion APIDTarget { set { m_APIDTarget = value; } } + public override bool APIDActive { set { m_useAPID = value; } } + public override float APIDStrength { set { m_APIDStrength = value; } } + public override float APIDDamping { set { m_APIDDamping = value; } } public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } public override bool PIDHoverActive { set { m_useHoverPID = value; } } diff --git a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs index 35fc616..135f49e 100644 --- a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs +++ b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs @@ -307,6 +307,27 @@ namespace OpenSim.Region.Physics.POSPlugin { set { return; } } + + public override Quaternion APIDTarget + { + set { return; } + } + + public override bool APIDActive + { + set { return; } + } + + public override float APIDStrength + { + set { return; } + } + + public override float APIDDamping + { + set { return; } + } + public override void SubscribeEvents(int ms) { diff --git a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs index b50364b..4521dce 100644 --- a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs +++ b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs @@ -302,6 +302,26 @@ namespace OpenSim.Region.Physics.POSPlugin { set { return; } } + public override Quaternion APIDTarget + { + set { return; } + } + + public override bool APIDActive + { + set { return; } + } + + public override float APIDStrength + { + set { return; } + } + + public override float APIDDamping + { + set { return; } + } + public override void SubscribeEvents(int ms) { diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs index e7d989c..b3bf70b 100644 --- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs +++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs @@ -500,6 +500,28 @@ namespace OpenSim.Region.Physics.PhysXPlugin public override bool PIDHoverActive { set { return; } } public override PIDHoverType PIDHoverType { set { return; } } public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget + { + set { return; } + } + + public override bool APIDActive + { + set { return; } + } + + public override float APIDStrength + { + set { return; } + } + + public override float APIDDamping + { + set { return; } + } + + public override void SubscribeEvents(int ms) { @@ -782,6 +804,28 @@ namespace OpenSim.Region.Physics.PhysXPlugin public override bool PIDHoverActive { set { return; } } public override PIDHoverType PIDHoverType { set { return; } } public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget + { + set { return; } + } + + public override bool APIDActive + { + set { return; } + } + + public override float APIDStrength + { + set { return; } + } + + public override float APIDDamping + { + set { return; } + } + + public override void SubscribeEvents(int ms) { -- cgit v1.1 From a81a16f22f7af6d102f365f69a50c5d201ae8362 Mon Sep 17 00:00:00 2001 From: KittoFlora Date: Tue, 27 Oct 2009 19:56:39 +0100 Subject: llRotLookAt Pt 2 --- OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs | 34 ++++++++------- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 55 +++++++++++++++---------- 2 files changed, 52 insertions(+), 37 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs index 8b57f06..0c168c6 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs @@ -1,16 +1,4 @@ /* - * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - * * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. * @@ -35,6 +23,19 @@ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + * */ using System; @@ -66,8 +67,8 @@ namespace OpenSim.Region.Physics.OdePlugin // private OdeScene m_parentScene = null; private IntPtr m_body = IntPtr.Zero; - private IntPtr m_jointGroup = IntPtr.Zero; - private IntPtr m_aMotor = IntPtr.Zero; +// private IntPtr m_jointGroup = IntPtr.Zero; +// private IntPtr m_aMotor = IntPtr.Zero; // Vehicle properties @@ -560,6 +561,7 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body */ +//if(frcount == 0) Console.WriteLine("MoveAngular "); // Get what the body is doing, this includes 'external' influences d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); @@ -614,7 +616,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Error is 0 (no error) to +/- 2 (max error) // scale it by VAservo verterr = verterr * VAservo; -//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); +if(frcount == 0) Console.WriteLine("VAerr=" + verterr); // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. @@ -635,7 +637,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Deflection section tba // Sum velocities - m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection + m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) { diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index e1bf996..93c9a44 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -87,9 +87,6 @@ namespace OpenSim.Region.Physics.OdePlugin private Quaternion m_APIDTarget = new Quaternion(); private float m_APIDStrength = 0.5f; private float m_APIDDamping = 0.5f; - - private float APID_D = 35f; - private float APID_G = 25f; private bool m_useAPID = false; // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), @@ -192,6 +189,9 @@ namespace OpenSim.Region.Physics.OdePlugin private ODEDynamics m_vehicle; internal int m_material = (int)Material.Wood; + + private int frcount = 0; // Used to limit dynamics debug output to + public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size, Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) @@ -1563,9 +1563,14 @@ Console.WriteLine(" JointCreateFixed"); float fy = 0; float fz = 0; + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. { +if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type + + " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); if (m_vehicle.Type != Vehicle.TYPE_NONE) { // 'VEHICLES' are dealt with in ODEDynamics.cs @@ -1573,7 +1578,6 @@ Console.WriteLine(" JointCreateFixed"); } else { -//Console.WriteLine("Move " + m_primName); if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 // NON-'VEHICLES' are dealt with here if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f)) @@ -1602,7 +1606,7 @@ Console.WriteLine(" JointCreateFixed"); if (m_usePID) { -//Console.WriteLine("PID " + m_primName); +//if(frcount == 0) Console.WriteLine("PID " + m_primName); // KF - this is for object MoveToTarget. //if (!d.BodyIsEnabled(Body)) @@ -1741,7 +1745,7 @@ Console.WriteLine(" JointCreateFixed"); d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); d.BodySetLinearVel(Body, vel.X, vel.Y, 0); d.BodyAddForce(Body, 0, 0, fz); - return; + //KF this prevents furthur motions return; } else { @@ -1756,13 +1760,16 @@ Console.WriteLine(" JointCreateFixed"); { // RotLookAt, apparently overrides all other rotation sources. Inputs: // Quaternion m_APIDTarget - // float m_APIDStrength // perhaps ratio other forces to lookat force? - // float m_APIDDamping //'seconds to critically damps in'[sic] + // float m_APIDStrength // From SL experiments, this is the time to get there + // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly + // Also in SL the mass of the object has no effect on time to get there. // Factors: - // float APID_D - // float APID_G - +//if(frcount == 0) Console.WriteLine("APID "); // get present body rotation + float limit = 1.0f; + float scaler = 50f; // adjusts damping time + float RLAservo = 0f; + d.Quaternion rot = d.BodyGetQuaternion(Body); Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; @@ -1770,15 +1777,21 @@ Console.WriteLine(" JointCreateFixed"); Vector3 diff_axis; rot_diff.GetAxisAngle(out diff_axis, out diff_angle); diff_axis.Normalize(); - PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); - float RLAservo = timestep / m_APIDDamping; - rotforce = rotforce * RLAservo * m_mass; - d.BodyAddTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); - - // d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); - - - + if(diff_angle > 0.01f) // diff_angle is always +ve + { +// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if(diff_angle > limit) diff_angle = limit; // cap the rotate rate +// RLAservo = timestep / m_APIDStrength * m_mass * scaler; + // rotforce = rotforce * RLAservo * diff_angle ; + // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); + RLAservo = timestep / m_APIDStrength * scaler; + rotforce = rotforce * RLAservo * diff_angle ; + d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); +//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } +if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); } // end m_useAPID fx *= m_mass; @@ -2674,7 +2687,7 @@ Console.WriteLine(" JointCreateFixed"); //outofBounds = true; } - float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); +// float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); //Console.WriteLine("Adiff " + m_primName + " = " + Adiff); if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) -- cgit v1.1 From 4c10826caa4b2b19200ab3f0272bb1c20c26c53e Mon Sep 17 00:00:00 2001 From: KittoFlora Date: Mon, 16 Nov 2009 02:12:56 +0100 Subject: Fix merge conflicts --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 6 +++--- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 7 ------- 2 files changed, 3 insertions(+), 10 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 8241e8f..b82586f 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -249,9 +249,9 @@ namespace OpenSim.Region.Physics.Manager public abstract float APIDStrength { set;} public abstract float APIDDamping { set;} - public abstract void AddForce(PhysicsVector force, bool pushforce); - public abstract void AddAngularForce(PhysicsVector force, bool pushforce); - public abstract void SetMomentum(PhysicsVector momentum); + public abstract void AddForce(Vector3 force, bool pushforce); + public abstract void AddAngularForce(Vector3 force, bool pushforce); + public abstract void SetMomentum(Vector3 momentum); public abstract void SubscribeEvents(int ms); public abstract void UnSubscribeEvents(); public abstract bool SubscribedEvents(); diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index b963d65..8f2c801 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -107,17 +107,10 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_PIDHoverTau; private bool m_useHoverPID; private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; -<<<<<<< HEAD:OpenSim/Region/Physics/OdePlugin/ODEPrim.cs - private float m_targetHoverHeight = 0f; - private float m_groundHeight = 0f; - private float m_waterHeight = 0f; - private float m_buoyancy = 0f; //Set by llSetBuoyancy(), for non-vehicles. -======= private float m_targetHoverHeight; private float m_groundHeight; private float m_waterHeight; private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. ->>>>>>> vehicles:OpenSim/Region/Physics/OdePlugin/ODEPrim.cs // private float m_tensor = 5f; private int body_autodisable_frames = 20; -- cgit v1.1 From 7f0f5060ec418533ee7d952bdc0815f3925bda9f Mon Sep 17 00:00:00 2001 From: KittoFlora Date: Thu, 19 Nov 2009 20:13:26 +0100 Subject: Clean up messages in ODE --- OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs | 2 +- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs index 1842eb4..78b15be 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs @@ -629,7 +629,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Error is 0 (no error) to +/- 2 (max error) // scale it by VAservo verterr = verterr * VAservo; -if(frcount == 0) Console.WriteLine("VAerr=" + verterr); +//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index 8f2c801..2bf96e4 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -1584,8 +1584,8 @@ Console.WriteLine(" JointCreateFixed"); if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. { -if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type + - " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); +//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type + + // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); if (m_vehicle.Type != Vehicle.TYPE_NONE) { // 'VEHICLES' are dealt with in ODEDynamics.cs @@ -1806,7 +1806,7 @@ if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); } -if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); +//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); } // end m_useAPID fx *= m_mass; -- cgit v1.1 From 6483470ec5e9000fb6a85bbdfe5b79be6f336a74 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 17 Dec 2009 02:54:02 -0500 Subject: Fix GetWorldRotation(), and a host of related Sit fixes. --- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index c27c420..688be83 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -2673,7 +2673,7 @@ Console.WriteLine(" JointCreateFixed"); m_lastposition = _position; m_lastorientation = _orientation; - + l_position.X = vec.X; l_position.Y = vec.Y; l_position.Z = vec.Z; @@ -2681,6 +2681,10 @@ Console.WriteLine(" JointCreateFixed"); l_orientation.Y = ori.Y; l_orientation.Z = ori.Z; l_orientation.W = ori.W; + +// if(l_position.Y != m_lastposition.Y){ +// Console.WriteLine("UP&V {0} {1}", m_primName, l_position); +// } if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) { -- cgit v1.1 From 0a29842caf5dbe711490b9b323ae922c418c6c30 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Tue, 22 Dec 2009 00:20:04 -0500 Subject: Include ChOdePlugin --- OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs | 58 + OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 1353 +++++++ .../Physics/ChOdePlugin/ODEDynamics.c_comments | 630 ++++ OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 673 ++++ OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 3271 +++++++++++++++++ .../ChOdePlugin/ODERayCastRequestManager.cs | 375 ++ .../Region/Physics/ChOdePlugin/OdePhysicsJoint.cs | 48 + OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 3865 ++++++++++++++++++++ .../Physics/ChOdePlugin/Tests/ODETestClass.cs | 122 + OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs | 98 + OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 21 +- OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs | 20 +- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 108 +- 13 files changed, 10524 insertions(+), 118 deletions(-) create mode 100644 OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs create mode 100644 OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs create mode 100644 OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments create mode 100644 OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs create mode 100644 OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs create mode 100644 OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs create mode 100644 OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs create mode 100644 OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs create mode 100644 OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs create mode 100644 OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs new file mode 100644 index 0000000..d65929a --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs @@ -0,0 +1,58 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Reflection; +using System.Runtime.InteropServices; + +// Information about this assembly is defined by the following +// attributes. +// +// change them to the information which is associated with the assembly +// you compile. + +[assembly : AssemblyTitle("OdePlugin")] +[assembly : AssemblyDescription("")] +[assembly : AssemblyConfiguration("")] +[assembly : AssemblyCompany("http://opensimulator.org")] +[assembly : AssemblyProduct("OdePlugin")] +[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")] +[assembly : AssemblyTrademark("")] +[assembly : AssemblyCulture("")] + +// This sets the default COM visibility of types in the assembly to invisible. +// If you need to expose a type to COM, use [ComVisible(true)] on that type. + +[assembly : ComVisible(false)] + +// The assembly version has following format : +// +// Major.Minor.Build.Revision +// +// You can specify all values by your own or you can build default build and revision +// numbers with the '*' character (the default): + +[assembly : AssemblyVersion("0.6.5.*")] diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs new file mode 100644 index 0000000..aa0acb7 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -0,0 +1,1353 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using OpenMetaverse; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using log4net; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. + /// + + public enum dParam : int + { + LowStop = 0, + HiStop = 1, + Vel = 2, + FMax = 3, + FudgeFactor = 4, + Bounce = 5, + CFM = 6, + StopERP = 7, + StopCFM = 8, + LoStop2 = 256, + HiStop2 = 257, + Vel2 = 258, + FMax2 = 259, + StopERP2 = 7 + 256, + StopCFM2 = 8 + 256, + LoStop3 = 512, + HiStop3 = 513, + Vel3 = 514, + FMax3 = 515, + StopERP3 = 7 + 512, + StopCFM3 = 8 + 512 + } + public class OdeCharacter : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private Vector3 _position; + private d.Vector3 _zeroPosition; + // private d.Matrix3 m_StandUpRotation; + private bool _zeroFlag = false; + private bool m_lastUpdateSent = false; + private Vector3 _velocity; + private Vector3 _target_velocity; + private Vector3 _acceleration; + private Vector3 m_rotationalVelocity; + private float m_mass = 80f; + public float m_density = 60f; + private bool m_pidControllerActive = true; + public float PID_D = 800.0f; + public float PID_P = 900.0f; + //private static float POSTURE_SERVO = 10000.0f; + public float CAPSULE_RADIUS = 0.37f; + public float CAPSULE_LENGTH = 2.140599f; + public float m_tensor = 3800000f; + public float heightFudgeFactor = 0.52f; + public float walkDivisor = 1.3f; + public float runDivisor = 0.8f; + private bool flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_wascolliding = false; + private bool m_wascollidingGround = false; + private bool m_iscollidingObj = false; + private bool m_alwaysRun = false; + private bool m_hackSentFall = false; + private bool m_hackSentFly = false; + private int m_requestedUpdateFrequency = 0; + private Vector3 m_taintPosition = Vector3.Zero; + public uint m_localID = 0; + public bool m_returnCollisions = false; + // taints and their non-tainted counterparts + public bool m_isPhysical = false; // the current physical status + public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing) + public float MinimumGroundFlightOffset = 3f; + + private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. + private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f; // used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider + + + private float m_buoyancy = 0f; + + // private CollisionLocker ode; + + private string m_name = String.Empty; + + private bool[] m_colliderarr = new bool[11]; + private bool[] m_colliderGroundarr = new bool[11]; + + // Default we're a Character + private CollisionCategories m_collisionCategories = (CollisionCategories.Character); + + // Default, Collide with Other Geometries, spaces, bodies and characters. + private CollisionCategories m_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + | CollisionCategories.Land); + public IntPtr Body = IntPtr.Zero; + private OdeScene _parent_scene; + public IntPtr Shell = IntPtr.Zero; + public IntPtr Amotor = IntPtr.Zero; + public d.Mass ShellMass; + public bool collidelock = false; + + public int m_eventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + // unique UUID of this character object + public UUID m_uuid; + public bool bad = false; + + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor) + { + m_uuid = UUID.Random(); + + if (pos.IsFinite()) + { + if (pos.Z > 9999999f) + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (pos.Z < -90000f) + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + _position = pos; + m_taintPosition.X = pos.X; + m_taintPosition.Y = pos.Y; + m_taintPosition.Z = pos.Z; + } + else + { + _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); + m_taintPosition.X = _position.X; + m_taintPosition.Y = _position.Y; + m_taintPosition.Z = _position.Z; + m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); + } + + _parent_scene = parent_scene; + + PID_D = pid_d; + PID_P = pid_p; + CAPSULE_RADIUS = capsule_radius; + m_tensor = tensor; + m_density = density; + heightFudgeFactor = height_fudge_factor; + walkDivisor = walk_divisor; + runDivisor = rundivisor; + + // m_StandUpRotation = + // new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f, + // 0.5f); + + for (int i = 0; i < 11; i++) + { + m_colliderarr[i] = false; + } + CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH; + + m_isPhysical = false; // current status: no ODE information exists + m_tainted_isPhysical = true; // new tainted status: need to create ODE information + + _parent_scene.AddPhysicsActorTaint(this); + + m_name = avName; + } + + public override int PhysicsActorType + { + get { return (int) ActorTypes.Agent; } + set { return; } + } + + /// + /// If this is set, the avatar will move faster + /// + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + + public override uint LocalID + { + set { m_localID = value; } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set { return; } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override bool FloatOnWater + { + set { return; } + } + + public override bool IsPhysical + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + + public override bool Flying + { + get { return flying; } + set { flying = value; } + } + + /// + /// Returns if the avatar is colliding in general. + /// This includes the ground and objects and avatar. + /// + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + int i; + int truecount = 0; + int falsecount = 0; + + if (m_colliderarr.Length >= 10) + { + for (i = 0; i < 10; i++) + { + m_colliderarr[i] = m_colliderarr[i + 1]; + } + } + m_colliderarr[10] = value; + + for (i = 0; i < 11; i++) + { + if (m_colliderarr[i]) + { + truecount++; + } + else + { + falsecount++; + } + } + + // Equal truecounts and false counts means we're colliding with something. + + if (falsecount > 1.2*truecount) + { + m_iscolliding = false; + } + else + { + m_iscolliding = true; + } + if (m_wascolliding != m_iscolliding) + { + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + m_wascolliding = m_iscolliding; + } + } + + /// + /// Returns if an avatar is colliding with the ground + /// + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { + // Collisions against the ground are not really reliable + // So, to get a consistant value we have to average the current result over time + // Currently we use 1 second = 10 calls to this. + int i; + int truecount = 0; + int falsecount = 0; + + if (m_colliderGroundarr.Length >= 10) + { + for (i = 0; i < 10; i++) + { + m_colliderGroundarr[i] = m_colliderGroundarr[i + 1]; + } + } + m_colliderGroundarr[10] = value; + + for (i = 0; i < 11; i++) + { + if (m_colliderGroundarr[i]) + { + truecount++; + } + else + { + falsecount++; + } + } + + // Equal truecounts and false counts means we're colliding with something. + + if (falsecount > 1.2*truecount) + { + m_iscollidingGround = false; + } + else + { + m_iscollidingGround = true; + } + if (m_wascollidingGround != m_iscollidingGround) + { + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + m_wascollidingGround = m_iscollidingGround; + } + } + + /// + /// Returns if the avatar is colliding with an object + /// + public override bool CollidingObj + { + get { return m_iscollidingObj; } + set + { + m_iscollidingObj = value; + if (value) + m_pidControllerActive = false; + else + m_pidControllerActive = true; + } + } + + /// + /// turn the PID controller on or off. + /// The PID Controller will turn on all by itself in many situations + /// + /// + public void SetPidStatus(bool status) + { + m_pidControllerActive = status; + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + /// + /// This 'puts' an avatar somewhere in the physics space. + /// Not really a good choice unless you 'know' it's a good + /// spot otherwise you're likely to orbit the avatar. + /// + public override Vector3 Position + { + get { return _position; } + set + { + if (Body == IntPtr.Zero || Shell == IntPtr.Zero) + { + if (value.IsFinite()) + { + if (value.Z > 9999999f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (value.Z < -90000f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + + _position.X = value.X; + _position.Y = value.Y; + _position.Z = value.Z; + + m_taintPosition.X = value.X; + m_taintPosition.Y = value.Y; + m_taintPosition.Z = value.Z; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); + } + } + } + } + + public override Vector3 RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + + /// + /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight + /// and use it to offset landings properly + /// + public override Vector3 Size + { + get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); } + set + { + if (value.IsFinite()) + { + m_pidControllerActive = true; + + Vector3 SetSize = value; + m_tainted_CAPSULE_LENGTH = (SetSize.Z*1.15f) - CAPSULE_RADIUS*2.0f; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + + Velocity = Vector3.Zero; + + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); + } + } + } + + private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector) + { + movementVector.Z = 0f; + float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y)); + if (magnitude < 0.1f) return; + + // normalize the velocity vector + float invMagnitude = 1.0f / magnitude; + movementVector.X *= invMagnitude; + movementVector.Y *= invMagnitude; + + // if we change the capsule heading too often, the capsule can fall down + // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw), + // meaning only 4 possible capsule tilt orientations + if (movementVector.X > 0) + { + // east + if (movementVector.Y > 0) + { + // northeast + movementVector.X = (float)Math.Sqrt(2.0); + movementVector.Y = (float)Math.Sqrt(2.0); + } + else + { + // southeast + movementVector.X = (float)Math.Sqrt(2.0); + movementVector.Y = -(float)Math.Sqrt(2.0); + } + } + else + { + // west + if (movementVector.Y > 0) + { + // northwest + movementVector.X = -(float)Math.Sqrt(2.0); + movementVector.Y = (float)Math.Sqrt(2.0); + } + else + { + // southwest + movementVector.X = -(float)Math.Sqrt(2.0); + movementVector.Y = -(float)Math.Sqrt(2.0); + } + } + + + // movementVector.Z is zero + + // calculate tilt components based on desired amount of tilt and current (snapped) heading. + // the "-" sign is to force the tilt to be OPPOSITE the direction of movement. + float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane; + float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane; + + //m_log.Debug("[PHYSICS] changing avatar tilt"); + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop + } + + /// + /// This creates the Avatar's physical Surrogate at the position supplied + /// + /// + /// + /// + + // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access + // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only + // place that is safe to call this routine AvatarGeomAndBodyCreation. + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ, float tensor) + { + //CAPSULE_LENGTH = -5; + //CAPSULE_RADIUS = -5; + int dAMotorEuler = 1; + _parent_scene.waitForSpaceUnlock(_parent_scene.space); + if (CAPSULE_LENGTH <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_LENGTH = 0.01f; + + } + + if (CAPSULE_RADIUS <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_RADIUS = 0.01f; + + } + Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); + + d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); + d.GeomSetCollideBits(Shell, (int)m_collisionFlags); + + d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH); + Body = d.BodyCreate(_parent_scene.world); + d.BodySetPosition(Body, npositionX, npositionY, npositionZ); + + _position.X = npositionX; + _position.Y = npositionY; + _position.Z = npositionZ; + + + m_taintPosition.X = npositionX; + m_taintPosition.Y = npositionY; + m_taintPosition.Z = npositionZ; + + d.BodySetMass(Body, ref ShellMass); + d.Matrix3 m_caprot; + // 90 Stand up on the cap of the capped cyllinder + if (_parent_scene.IsAvCapsuleTilted) + { + d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2)); + } + else + { + d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2)); + } + + + d.GeomSetRotation(Shell, ref m_caprot); + d.BodySetRotation(Body, ref m_caprot); + + d.GeomSetBody(Shell, Body); + + + // The purpose of the AMotor here is to keep the avatar's physical + // surrogate from rotating while moving + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + d.JointSetAMotorMode(Amotor, dAMotorEuler); + d.JointSetAMotorNumAxes(Amotor, 3); + d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); + d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); + d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorAngle(Amotor, 1, 0); + d.JointSetAMotorAngle(Amotor, 2, 0); + + // These lowstops and high stops are effectively (no wiggle room) + if (_parent_scene.IsAvCapsuleTilted) + { + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f); + } + else + { + #region Documentation of capsule motor LowStop and HighStop parameters + // Intentionally introduce some tilt into the capsule by setting + // the motor stops to small epsilon values. This small tilt prevents + // the capsule from falling into the terrain; a straight-up capsule + // (with -0..0 motor stops) falls into the terrain for reasons yet + // to be comprehended in their entirety. + #endregion + AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero); + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.08f); // must be same as lowstop, else a different, spurious tilt is introduced + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop + } + + // Fudge factor is 1f by default, we're setting it to 0. We don't want it to Fudge or the + // capped cyllinder will fall over + d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor); + + //d.Matrix3 bodyrotation = d.BodyGetRotation(Body); + //d.QfromR( + //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068, + // + //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22); + //standupStraight(); + } + + // + /// + /// Uses the capped cyllinder volume formula to calculate the avatar's mass. + /// This may be used in calculations in the scene/scenepresence + /// + public override float Mass + { + get + { + float AVvolume = (float) (Math.PI*Math.Pow(CAPSULE_RADIUS, 2)*CAPSULE_LENGTH); + return m_density*AVvolume; + } + } + public override void link(PhysicsActor obj) + { + + } + + public override void delink() + { + + } + + public override void LockAngularMotion(Vector3 axis) + { + + } + +// This code is very useful. Written by DanX0r. We're just not using it right now. +// Commented out to prevent a warning. +// +// private void standupStraight() +// { +// // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air. +// // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you +// // change appearance and when you enter the simulator +// // After this routine is done, the amotor stabilizes much quicker +// d.Vector3 feet; +// d.Vector3 head; +// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet); +// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head); +// float posture = head.Z - feet.Z; + +// // restoring force proportional to lack of posture: +// float servo = (2.5f - posture) * POSTURE_SERVO; +// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f); +// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f); +// //d.Matrix3 bodyrotation = d.BodyGetRotation(Body); +// //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22); +// } + + public override Vector3 Force + { + get { return _target_velocity; } + set { return; } + } + + public override int VehicleType + { + get { return 0; } + set { return; } + } + + public override void VehicleFloatParam(int param, float value) + { + + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + + } + + public override void SetVolumeDetect(int param) + { + + } + + public override Vector3 CenterOfMass + { + get { return Vector3.Zero; } + } + + public override Vector3 GeometricCenter + { + get { return Vector3.Zero; } + } + + public override PrimitiveBaseShape Shape + { + set { return; } + } + + public override Vector3 Velocity + { + get { + // There's a problem with Vector3.Zero! Don't Use it Here! + if (_zeroFlag) + return Vector3.Zero; + m_lastUpdateSent = false; + return _velocity; + } + set + { + if (value.IsFinite()) + { + m_pidControllerActive = true; + _target_velocity = value; + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); + } + } + } + + public override Vector3 Torque + { + get { return Vector3.Zero; } + set { return; } + } + + public override float CollisionScore + { + get { return 0f; } + set { } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return Quaternion.Identity; } + set { + //Matrix3 or = Orientation.ToRotationMatrix(); + //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22); + //d.BodySetRotation(Body, ref ord); + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + } + + public void SetAcceleration(Vector3 accel) + { + m_pidControllerActive = true; + _acceleration = accel; + } + + /// + /// Adds the force supplied to the Target Velocity + /// The PID controller takes this target velocity and tries to make it a reality + /// + /// + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + if (pushforce) + { + m_pidControllerActive = false; + force *= 100f; + doForce(force); + // If uncommented, things get pushed off world + // + // m_log.Debug("Push!"); + // _target_velocity.X += force.X; + // _target_velocity.Y += force.Y; + // _target_velocity.Z += force.Z; + } + else + { + m_pidControllerActive = true; + _target_velocity.X += force.X; + _target_velocity.Y += force.Y; + _target_velocity.Z += force.Z; + } + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character"); + } + //m_lastUpdateSent = false; + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + + } + + /// + /// After all of the forces add up with 'add force' we apply them with doForce + /// + /// + public void doForce(Vector3 force) + { + if (!collidelock) + { + d.BodyAddForce(Body, force.X, force.Y, force.Z); + //d.BodySetRotation(Body, ref m_StandUpRotation); + //standupStraight(); + + } + } + + public override void SetMomentum(Vector3 momentum) + { + } + + + /// + /// Called from Simulate + /// This is the avatar's movement control + PID Controller + /// + /// + public void Move(float timeStep, List defects) + { + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if (Body == IntPtr.Zero) + return; + + if (m_pidControllerActive == false) + { + _zeroPosition = d.BodyGetPosition(Body); + } + //PidStatus = true; + + d.Vector3 localpos = d.BodyGetPosition(Body); + Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z); + + if (!localPos.IsFinite()) + { + + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + + // destroy avatar capsule and related ODE data + if (Amotor != IntPtr.Zero) + { + // Kill the Amotor + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + //kill the Geometry + _parent_scene.waitForSpaceUnlock(_parent_scene.space); + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + + Body = IntPtr.Zero; + } + + if (Shell != IntPtr.Zero) + { + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } + + return; + } + + Vector3 vec = Vector3.Zero; + d.Vector3 vel = d.BodyGetLinearVel(Body); + + float movementdivisor = 1f; + + if (!m_alwaysRun) + { + movementdivisor = walkDivisor; + } + else + { + movementdivisor = runDivisor; + } + + // if velocity is zero, use position control; otherwise, velocity control + if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!_zeroFlag) + { + _zeroFlag = true; + _zeroPosition = d.BodyGetPosition(Body); + } + if (m_pidControllerActive) + { + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.Vector3 pos = d.BodyGetPosition(Body); + vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2); + if (flying) + { + vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + } + } + //PidStatus = true; + } + else + { + m_pidControllerActive = true; + _zeroFlag = false; + if (m_iscolliding && !flying) + { + // We're standing on something + vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D); + vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D); + } + else if (m_iscolliding && flying) + { + // We're flying and colliding with something + vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16); + vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16); + } + else if (!m_iscolliding && flying) + { + // we're in mid air suspended + vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6); + vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6); + } + + if (m_iscolliding && !flying && _target_velocity.Z > 0.0f) + { + // We're colliding with something and we're not flying but we're moving + // This means we're walking or running. + d.Vector3 pos = d.BodyGetPosition(Body); + vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P; + if (_target_velocity.X > 0) + { + vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D; + } + if (_target_velocity.Y > 0) + { + vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D; + } + } + else if (!m_iscolliding && !flying) + { + // we're not colliding and we're not flying so that means we're falling! + // m_iscolliding includes collisions with the ground. + + // d.Vector3 pos = d.BodyGetPosition(Body); + if (_target_velocity.X > 0) + { + vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D; + } + if (_target_velocity.Y > 0) + { + vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D; + } + } + + if (flying) + { + vec.Z = (_target_velocity.Z - vel.Z) * (PID_D); + } + } + if (flying) + { + vec.Z += ((-1 * _parent_scene.gravityz)*m_mass); + + //Added for auto fly height. Kitto Flora + //d.Vector3 pos = d.BodyGetPosition(Body); + float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset; + + if (_position.Z < target_altitude) + { + vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f; + } + // end add Kitto Flora + } + if (vec.IsFinite()) + { + doForce(vec); + if (!_zeroFlag) + { + AlignAvatarTiltWithCurrentDirectionOfMovement(vec); + } + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + if (Amotor != IntPtr.Zero) + { + // Kill the Amotor + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + //kill the Geometry + _parent_scene.waitForSpaceUnlock(_parent_scene.space); + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + + Body = IntPtr.Zero; + } + + if (Shell != IntPtr.Zero) + { + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } + } + } + + /// + /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. + /// + public void UpdatePositionAndVelocity() + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + d.Vector3 vec; + try + { + vec = d.BodyGetPosition(Body); + } + catch (NullReferenceException) + { + bad = true; + _parent_scene.BadCharacter(this); + vec = new d.Vector3(_position.X, _position.Y, _position.Z); + base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem! + m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid); + } + + + // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) + if (vec.X < 0.0f) vec.X = 0.0f; + if (vec.Y < 0.0f) vec.Y = 0.0f; + if (vec.X > (int)_parent_scene.WorldExtents.X - 0.05f) vec.X = (int)_parent_scene.WorldExtents.X - 0.05f; + if (vec.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) vec.Y = (int)_parent_scene.WorldExtents.Y - 0.05f; + + _position.X = vec.X; + _position.Y = vec.Y; + _position.Z = vec.Z; + + // Did we move last? = zeroflag + // This helps keep us from sliding all over + + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + // Did we send out the 'stopped' message? + if (!m_lastUpdateSent) + { + m_lastUpdateSent = true; + //base.RequestPhysicsterseUpdate(); + + } + } + else + { + m_lastUpdateSent = false; + try + { + vec = d.BodyGetLinearVel(Body); + } + catch (NullReferenceException) + { + vec.X = _velocity.X; + vec.Y = _velocity.Y; + vec.Z = _velocity.Z; + } + _velocity.X = (vec.X); + _velocity.Y = (vec.Y); + + _velocity.Z = (vec.Z); + + if (_velocity.Z < -6 && !m_hackSentFall) + { + m_hackSentFall = true; + m_pidControllerActive = false; + } + else if (flying && !m_hackSentFly) + { + //m_hackSentFly = true; + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + else + { + m_hackSentFly = false; + m_hackSentFall = false; + } + } + } + + /// + /// Cleanup the things we use in the scene. + /// + public void Destroy() + { + m_tainted_isPhysical = false; + _parent_scene.AddPhysicsActorTaint(this); + } + + public override void CrossingFailure() + { + } + + public override Vector3 PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } + + public override float PIDHoverHeight { set { return; } } + public override bool PIDHoverActive { set { return; } } + public override PIDHoverType PIDHoverType { set { return; } } + public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget{ set { return; } } + + public override bool APIDActive{ set { return; } } + + public override float APIDStrength{ set { return; } } + + public override float APIDDamping{ set { return; } } + + + public override void SubscribeEvents(int ms) + { + m_requestedUpdateFrequency = ms; + m_eventsubscription = ms; + _parent_scene.addCollisionEventReporting(this); + } + public override void UnSubscribeEvents() + { + _parent_scene.remCollisionEventReporting(this); + m_requestedUpdateFrequency = 0; + m_eventsubscription = 0; + } + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (m_eventsubscription > 0) + { + CollisionEventsThisFrame.addCollider(CollidedWith, contact); + } + } + + public void SendCollisions() + { + if (m_eventsubscription > m_requestedUpdateFrequency) + { + if (CollisionEventsThisFrame != null) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + } + CollisionEventsThisFrame = new CollisionEventUpdate(); + m_eventsubscription = 0; + } + } + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + public void ProcessTaints(float timestep) + { + + if (m_tainted_isPhysical != m_isPhysical) + { + if (m_tainted_isPhysical) + { + // Create avatar capsule and related ODE data + if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero)) + { + m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " + + (Shell!=IntPtr.Zero ? "Shell ":"") + + (Body!=IntPtr.Zero ? "Body ":"") + + (Amotor!=IntPtr.Zero ? "Amotor ":"")); + } + AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor); + + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + _parent_scene.AddCharacter(this); + } + else + { + _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + if (Amotor != IntPtr.Zero) + { + // Kill the Amotor + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + //kill the Geometry + _parent_scene.waitForSpaceUnlock(_parent_scene.space); + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + + Body = IntPtr.Zero; + } + + if (Shell != IntPtr.Zero) + { + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } + + } + + m_isPhysical = m_tainted_isPhysical; + } + + if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) + { + if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + { + + m_pidControllerActive = true; + // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate() + d.JointDestroy(Amotor); + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + d.BodyDestroy(Body); + d.GeomDestroy(Shell); + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor); + Velocity = Vector3.Zero; + + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell==IntPtr.Zero ? "Shell ":"") + + (Body==IntPtr.Zero ? "Body ":"") + + (Amotor==IntPtr.Zero ? "Amotor ":"")); + } + } + + if (!m_taintPosition.ApproxEquals(_position, 0.05f)) + { + if (Body != IntPtr.Zero) + { + d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); + + _position.X = m_taintPosition.X; + _position.Y = m_taintPosition.Y; + _position.Z = m_taintPosition.Z; + } + } + + } + + internal void AddCollisionFrameTime(int p) + { + // protect it from overflow crashing + if (m_eventsubscription + p >= int.MaxValue) + m_eventsubscription = 0; + m_eventsubscription += p; + } + } +} diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments new file mode 100644 index 0000000..1060aa6 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments @@ -0,0 +1,630 @@ +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + * + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + + public IntPtr Body + { + get { return m_body; } + } + + private int frcount = 0; // Used to limit dynamics debug output to + // every 100th frame + + // private OdeScene m_parentScene = null; + private IntPtr m_body = IntPtr.Zero; + private IntPtr m_jointGroup = IntPtr.Zero; + private IntPtr m_aMotor = IntPtr.Zero; + + + // Vehicle properties + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + private Vector3 m_linearFrictionTimescale = Vector3.Zero; + private float m_linearMotorDecayTimescale = 0; + private float m_linearMotorTimescale = 0; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + // private bool m_LinearMotorSetLastFrame = false; + // private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; + private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; + private Vector3 m_angularFrictionTimescale = Vector3.Zero; + private float m_angularMotorDecayTimescale = 0; + private float m_angularMotorTimescale = 0; + private Vector3 m_lastAngularVelocityVector = Vector3.Zero; + + //Deflection properties + // private float m_angularDeflectionEfficiency = 0; + // private float m_angularDeflectionTimescale = 0; + // private float m_linearDeflectionEfficiency = 0; + // private float m_linearDeflectionTimescale = 0; + + //Banking properties + // private float m_bankingEfficiency = 0; + // private float m_bankingMix = 0; + // private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 0f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 0; + private float m_verticalAttractionTimescale = 0; + + + + + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0.0f) pValue = 0.0f; + if (pValue > 1.0f) pValue = 1.0f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + break; + + } + + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + // m_referenceFrame = pValue; + break; + } + + }//end ProcessRotationVehicleParam + + internal void ProcessTypeChange(Vehicle pType) + { +Console.WriteLine("ProcessTypeChange to " + pType); + + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 1; + // m_linearDeflectionTimescale = 1; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 1; + // m_bankingTimescale = 10; + // m_referenceFrame = Quaternion.Identity; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // // m_linearDeflectionEfficiency = 1; + // // m_linearDeflectionTimescale = 2; + // // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 10; + // m_bankingEfficiency = -0.2f; + // m_bankingMix = 1; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10,10,10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 0.5f; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5; + // m_bankingEfficiency = -0.3f; + // m_bankingMix = 0.8f; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2; + // m_bankingEfficiency = 1; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 2; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0; + // m_linearDeflectionTimescale = 5; + // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 5; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + + } + }//end SetDefaultsForType + + internal void Enable(IntPtr pBody, OdeScene pParentScene) + { +//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy); + if (m_type == Vehicle.TYPE_NONE) + return; + + m_body = pBody; + //KF: This used to set up the linear and angular joints + } + + internal void Step(float pTimestep, OdeScene pParentScene) + { + if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) + return; + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; + + MoveLinear(pTimestep, pParentScene); + MoveAngular(pTimestep); + }// end Step + + private void MoveLinear(float pTimestep, OdeScene _pParentScene) + { + if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + + // add drive to body + Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); + m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? + + // This will work temporarily, but we really need to compare speed on an axis + // KF: Limit body velocity to applied velocity? + if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) + m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; + if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) + m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) + m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; + + // decay applied velocity + Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_linearMotorDirection -= m_linearMotorDirection * decayfraction; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + else + { // requested is not significant + // if what remains of applied is small, zero it. + if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) + m_lastLinearVelocityVector = Vector3.Zero; + } + + + // convert requested object velocity to world-referenced vector + m_dir = m_lastLinearVelocityVector; + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + m_dir *= rotq; // apply obj rotation to velocity vector + + // add Gravity andBuoyancy + // KF: So far I have found no good method to combine a script-requested + // .Z velocity and gravity. Therefore only 0g will used script-requested + // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. + Vector3 grav = Vector3.Zero; + if(m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); + // Preserve the current Z velocity + d.Vector3 vel_now = d.BodyGetLinearVel(Body); + m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity + } // else its 1.0, no gravity. + + // Check if hovering + if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + +// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped +// m_VhoverTimescale = 0f; // time to acheive height +// pTimestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; +//if(frcount == 0) Console.WriteLine("herr0=" + herr0); + // Replace Vertical speed with correction figure if significant + if(Math.Abs(herr0) > 0.01f ) + { + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); + // m_VhoverEfficiency is not yet implemented + } + else + { + m_dir.Z = 0f; + } + } + + // Apply velocity + d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); +//if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); +//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z); + + + // apply friction + Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); + m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; + } // end MoveLinear() + + private void MoveAngular(float pTimestep) + { + + // m_angularMotorDirection is the latest value from the script, and is decayed here + // m_angularMotorDirectionLASTSET is the latest value from the script + // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here + + if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + // ramp up to new value + Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); + m_lastAngularVelocityVector += (addAmount * 10f); +//if(frcount == 0) Console.WriteLine("add: " + addAmount); + + // limit applied value to what was set by script + // This will work temporarily, but we really need to compare speed on an axis + if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X)) + m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; + if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y)) + m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) + m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; + + // decay the requested value + Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_angularMotorDirection -= m_angularMotorDirection * decayfraction; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ? + + // Vertical attractor section + +// d.Mass objMass; +// d.BodyGetMass(Body, out objMass); +// float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); + float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); + // get present body rotation + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by servo + verterr = verterr * servo; + + // rotate to object frame + // verterr = verterr * rotq; + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + m_lastAngularVelocityVector.X += verterr.Y; + m_lastAngularVelocityVector.Y -= verterr.X; +/* +if(frcount == 0) + { +// Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector); + Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}", + Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency)); + } + */ + d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z); + // apply friction + Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); + m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; + + } //end MoveAngular + } +} diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs new file mode 100644 index 0000000..78b15be --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs @@ -0,0 +1,673 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + * + */ + +/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + + public IntPtr Body + { + get { return m_body; } + } + + private int frcount = 0; // Used to limit dynamics debug output to + // every 100th frame + + // private OdeScene m_parentScene = null; + private IntPtr m_body = IntPtr.Zero; +// private IntPtr m_jointGroup = IntPtr.Zero; +// private IntPtr m_aMotor = IntPtr.Zero; + + + // Vehicle properties + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + private Vector3 m_linearFrictionTimescale = Vector3.Zero; + private float m_linearMotorDecayTimescale = 0; + private float m_linearMotorTimescale = 0; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + // private bool m_LinearMotorSetLastFrame = false; + // private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private int m_angularMotorApply = 0; // application frame counter + private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity + private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body + + //Deflection properties + // private float m_angularDeflectionEfficiency = 0; + // private float m_angularDeflectionTimescale = 0; + // private float m_linearDeflectionEfficiency = 0; + // private float m_linearDeflectionTimescale = 0; + + //Banking properties + // private float m_bankingEfficiency = 0; + // private float m_bankingMix = 0; + // private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; +// private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 0f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped + private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. + + + + + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; +// case Vehicle.HOVER_EFFICIENCY: +// if (pValue < 0f) pValue = 0f; +// if (pValue > 1f) pValue = 1f; +// m_VhoverEfficiency = pValue; +// break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable + if (pValue > 1.0f) pValue = 1.0f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + m_angularMotorApply = 10; + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + break; + + } + + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; + if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; + if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; + if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; + if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; + if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; + m_angularMotorApply = 10; + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + // m_referenceFrame = pValue; + break; + } + + }//end ProcessRotationVehicleParam + + internal void ProcessTypeChange(Vehicle pType) + { + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 1; + // m_linearDeflectionTimescale = 1; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 1; + // m_bankingTimescale = 10; + // m_referenceFrame = Quaternion.Identity; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // // m_linearDeflectionEfficiency = 1; + // // m_linearDeflectionTimescale = 2; + // // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 10f; + // m_bankingEfficiency = -0.2f; + // m_bankingMix = 1; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10,10,10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 0.5f; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5f; + // m_bankingEfficiency = -0.3f; + // m_bankingMix = 0.8f; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + // m_bankingEfficiency = 1; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 2; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10; + m_VhoverHeight = 5; +// m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0; + // m_linearDeflectionTimescale = 5; + // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 100f; + // m_bankingEfficiency = 0; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 5; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + + } + }//end SetDefaultsForType + + internal void Enable(IntPtr pBody, OdeScene pParentScene) + { + if (m_type == Vehicle.TYPE_NONE) + return; + + m_body = pBody; + } + + internal void Step(float pTimestep, OdeScene pParentScene) + { + if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) + return; + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; + + MoveLinear(pTimestep, pParentScene); + MoveAngular(pTimestep); + }// end Step + + private void MoveLinear(float pTimestep, OdeScene _pParentScene) + { + if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + + // add drive to body + Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); + m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? + + // This will work temporarily, but we really need to compare speed on an axis + // KF: Limit body velocity to applied velocity? + if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) + m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; + if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) + m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) + m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; + + // decay applied velocity + Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + else + { // requested is not significant + // if what remains of applied is small, zero it. + if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) + m_lastLinearVelocityVector = Vector3.Zero; + } + + + // convert requested object velocity to world-referenced vector + m_dir = m_lastLinearVelocityVector; + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + m_dir *= rotq; // apply obj rotation to velocity vector + + // add Gravity and Buoyancy + // KF: So far I have found no good method to combine a script-requested + // .Z velocity and gravity. Therefore only 0g will used script-requested + // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. + Vector3 grav = Vector3.Zero; + if(m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); + // Preserve the current Z velocity + d.Vector3 vel_now = d.BodyGetLinearVel(Body); + m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity + } // else its 1.0, no gravity. + + // Check if hovering + if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + +// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped +// m_VhoverTimescale = 0f; // time to acheive height +// pTimestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; + // Replace Vertical speed with correction figure if significant + if(Math.Abs(herr0) > 0.01f ) + { + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); + //KF: m_VhoverEfficiency is not yet implemented + } + else + { + m_dir.Z = 0f; + } + } + + // Apply velocity + d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); + + + // apply friction + Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); + m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; + } // end MoveLinear() + + private void MoveAngular(float pTimestep) + { + /* + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private int m_angularMotorApply = 0; // application frame counter + private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) + private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + */ +//if(frcount == 0) Console.WriteLine("MoveAngular "); + + // Get what the body is doing, this includes 'external' influences + d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); + // Vector3 angularVelocity = Vector3.Zero; + + if (m_angularMotorApply > 0) + { + // ramp up to new value + // current velocity += error / ( time to get there / step interval ) + // requested speed - last motor speed + m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); + m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); + m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); + + m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected + // velocity may still be acheived. + } + else + { + // no motor recently applied, keep the body velocity + /* m_angularMotorVelocity.X = angularVelocity.X; + m_angularMotorVelocity.Y = angularVelocity.Y; + m_angularMotorVelocity.Z = angularVelocity.Z; */ + + // and decay the velocity + m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); + } // end motor section + + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if(m_verticalAttractionTimescale < 300) + { + float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep); + // get present body rotation + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by VAservo + verterr = verterr * VAservo; +//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = - verterr.X; + vertattr.Z = 0f; + + // scaling appears better usingsquare-law + float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); + vertattr.X += bounce * angularVelocity.X; + vertattr.Y += bounce * angularVelocity.Y; + + } // else vertical attractor is off + + // m_lastVertAttractor = vertattr; + + // Bank section tba + // Deflection section tba + + // Sum velocities + m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection + + if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + } + else + { + m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. + } + + // apply friction + Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); + m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; + + // Apply to the body + d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); + + } //end MoveAngular + } +} diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs new file mode 100644 index 0000000..983431d --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -0,0 +1,3271 @@ +/* Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using log4net; +using OpenMetaverse; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. + /// + + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _torque; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Quaternion m_lastorientation = new Quaternion(); + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 _acceleration; + // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); + private Quaternion _orientation; + private Vector3 m_taintposition; + private Vector3 m_taintsize; + private Vector3 m_taintVelocity; + private Vector3 m_taintTorque; + private Quaternion m_taintrot; + private Vector3 m_angularlock = Vector3.One; + private Vector3 m_taintAngularLock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private float PID_D = 35f; + private float PID_G = 25f; + private bool m_usePID = false; + + private Quaternion m_APIDTarget = new Quaternion(); + private float m_APIDStrength = 0.5f; + private float m_APIDDamping = 0.5f; + private bool m_useAPID = false; + + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private float m_targetHoverHeight; + private float m_groundHeight; + private float m_waterHeight; + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + + // private float m_tensor = 5f; + private int body_autodisable_frames = 20; + + + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + private bool m_taintshape; + private bool m_taintPhysics; + private bool m_collidesLand = true; + private bool m_collidesWater; + public bool m_returnCollisions; + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlags; + + public bool m_taintremove; + public bool m_taintdisable; + public bool m_disabled; + public bool m_taintadd; + public bool m_taintselected; + public bool m_taintCollidesWater; + + public uint m_localID; + + //public GCHandle gc; + private CollisionLocker ode; + + private bool m_taintforce = false; + private bool m_taintaddangularforce = false; + private Vector3 m_force; + private List m_forcelist = new List(); + private List m_angularforcelist = new List(); + + private IMesh _mesh; + private PrimitiveBaseShape _pbs; + private OdeScene _parent_scene; + public IntPtr m_targetSpace = IntPtr.Zero; + public IntPtr prim_geom; + public IntPtr prev_geom; + public IntPtr _triMeshData; + + private IntPtr _linkJointGroup = IntPtr.Zero; + private PhysicsActor _parent; + private PhysicsActor m_taintparent; + + private List childrenPrim = new List(); + + private bool iscolliding; + private bool m_isphysical; + private bool m_isSelected; + + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + private bool m_throttleUpdates; + private int throttleCounter; + public int m_interpenetrationcount; + public float m_collisionscore; + public int m_roundsUnderMotionThreshold; + private int m_crossingfailures; + + public bool outofBounds; + private float m_density = 10.000006836f; // Aluminum g/cm3; + + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body = IntPtr.Zero; + public String m_primName; + private Vector3 _target_velocity; + public d.Mass pMass; + + public int m_eventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame; + + private IntPtr m_linkJoint = IntPtr.Zero; + + public volatile bool childPrim; + + private ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + + private int frcount = 0; // Used to limit dynamics debug output to + + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) + { + m_vehicle = new ODEDynamics(); + //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); + ode = dode; + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); + } + _position = pos; + m_taintposition = pos; + PID_D = parent_scene.bodyPIDD; + PID_G = parent_scene.bodyPIDG; + m_density = parent_scene.geomDefaultDensity; + // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + + prim_geom = IntPtr.Zero; + prev_geom = IntPtr.Zero; + + if (!pos.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.Warn("[PHYSICS]: Got nonFinite Object create Size"); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + m_taintsize = _size; + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation"); + } + + _orientation = rotation; + m_taintrot = _orientation; + _mesh = mesh; + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = (IntPtr)0; + + if (pos.Z < 0) + m_isphysical = false; + else + { + m_isphysical = pisPhysical; + // If we're physical, we need to be in the master space for now. + // linksets *should* be in a space together.. but are not currently + if (m_isphysical) + m_targetSpace = _parent_scene.space; + } + m_primName = primName; + m_taintadd = true; + _parent_scene.AddPhysicsActorTaint(this); + // don't do .add() here; old geoms get recycled with the same hash + } + + public override int PhysicsActorType + { + get { return (int) ActorTypes.Prim; } + set { return; } + } + + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + + public override uint LocalID + { + set { + //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); + m_localID = value; } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set { + + + // This only makes the object not collidable if the object + // is physical or the object is modified somehow *IN THE FUTURE* + // without this, if an avatar selects prim, they can walk right + // through it while it's selected + m_collisionscore = 0; + if ((m_isphysical && !_zeroFlag) || !value) + { + m_taintselected = value; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_taintselected = value; + m_isSelected = value; + } + if(m_isSelected) disableBodySoft(); + } + } + + public void SetGeom(IntPtr geom) + { + prev_geom = prim_geom; + prim_geom = geom; +//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + if (childPrim) + { + if (_parent != null && _parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; +//Console.WriteLine("SetGeom calls ChildSetGeom"); + parent.ChildSetGeom(this); + } + } + //m_log.Warn("Setting Geom to: " + prim_geom); + } + + + + public void enableBodySoft() + { + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero) + { + d.BodyEnable(Body); + if (m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Enable(Body, _parent_scene); + } + + m_disabled = false; + } + } + + public void disableBodySoft() + { + m_disabled = true; + + if (m_isphysical && Body != IntPtr.Zero) + { + d.BodyDisable(Body); + } + } + + public void enableBody() + { + // Don't enable this body if we're a child prim + // this should be taken care of in the parent function not here + if (!childPrim) + { + // Sets the geom to a body + Body = d.BodyCreate(_parent_scene.world); + + setMass(); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.BodySetQuaternion(Body, ref myrot); + d.GeomSetBody(prim_geom, Body); + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode (Body, false); + + m_interpenetrationcount = 0; + m_collisionscore = 0; + m_disabled = false; + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + if (m_vehicle.Type != Vehicle.TYPE_NONE) + { + m_vehicle.Enable(Body, _parent_scene); + } + + _parent_scene.addActivePrim(this); + } + } + + #region Mass Calculation + + private float CalculateMass() + { + float volume = 0; + + // No material is passed to the physics engines yet.. soo.. + // we're using the m_density constant in the class definition + + float returnMass = 0; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // Profile Volume + + volume = _size.X*_size.Y*_size.Z; + + // If the user has 'hollowed out' + // ProfileHollow is one of those 0 to 50000 values :P + // we like percentages better.. so turning into a percentage + + if (((float) _pbs.ProfileHollow/50000f) > 0.0) + { + float hollowAmount = (float) _pbs.ProfileHollow/50000f; + + // calculate the hollow volume by it's shape compared to the prim shape + float hollowVolume = 0; + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + // Cube Hollow volume calculation + float hollowsizex = _size.X*hollowAmount; + float hollowsizey = _size.Y*hollowAmount; + float hollowsizez = _size.Z*hollowAmount; + hollowVolume = hollowsizex*hollowsizey*hollowsizez; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + float hRadius = _size.X/2; + float hLength = _size.Z; + + // pi * r2 * h + hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount); + break; + + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount); + break; + + default: + hollowVolume = 0; + break; + } + volume = volume - hollowVolume; + } + + break; + case ProfileShape.Circle: + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + // Cylinder + float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z); + float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z); + + // Approximating the cylinder's irregularity. + if (volume1 > volume2) + { + volume = (float)volume1 - (volume1 - volume2); + } + else if (volume2 > volume1) + { + volume = (float)volume2 - (volume2 - volume1); + } + else + { + // Regular cylinder + volume = volume1; + } + } + else + { + // We don't know what the shape is yet, so use default + volume = _size.X * _size.Y * _size.Z; + } + // If the user has 'hollowed out' + // ProfileHollow is one of those 0 to 50000 values :P + // we like percentages better.. so turning into a percentage + + if (((float)_pbs.ProfileHollow / 50000f) > 0.0) + { + float hollowAmount = (float)_pbs.ProfileHollow / 50000f; + + // calculate the hollow volume by it's shape compared to the prim shape + float hollowVolume = 0; + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + float hRadius = _size.X / 2; + float hLength = _size.Z; + + // pi * r2 * h + hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount); + break; + + case HollowShape.Square: + // Cube Hollow volume calculation + float hollowsizex = _size.X * hollowAmount; + float hollowsizey = _size.Y * hollowAmount; + float hollowsizez = _size.Z * hollowAmount; + hollowVolume = hollowsizex * hollowsizey * hollowsizez; + break; + + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); + break; + + default: + hollowVolume = 0; + break; + } + volume = volume - hollowVolume; + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + if (_size.X == _size.Y && _size.Y == _size.Z) + { + // regular sphere + // v = 4/3 * pi * r^3 + float sradius3 = (float)Math.Pow((_size.X / 2), 3); + volume = (float)((4f / 3f) * Math.PI * sradius3); + } + else + { + // we treat this as a box currently + volume = _size.X * _size.Y * _size.Z; + } + } + else + { + // We don't know what the shape is yet, so use default + volume = _size.X * _size.Y * _size.Z; + } + break; + + case ProfileShape.EquilateralTriangle: + /* + v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h + + // seed mesh + Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f); + Vertex PM = new Vertex(+0.5f, 0f, 0.0f); + Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f); + */ + float xA = -0.25f * _size.X; + float yA = -0.45f * _size.Y; + + float xB = 0.5f * _size.X; + float yB = 0; + + float xC = -0.25f * _size.X; + float yC = 0.45f * _size.Y; + + volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z); + + // If the user has 'hollowed out' + // ProfileHollow is one of those 0 to 50000 values :P + // we like percentages better.. so turning into a percentage + float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f); + if (((float)fhollowFactor / 50000f) > 0.0) + { + float hollowAmount = (float)fhollowFactor / 50000f; + + // calculate the hollow volume by it's shape compared to the prim shape + float hollowVolume = 0; + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); + break; + + case HollowShape.Square: + // Cube Hollow volume calculation + float hollowsizex = _size.X * hollowAmount; + float hollowsizey = _size.Y * hollowAmount; + float hollowsizez = _size.Z * hollowAmount; + hollowVolume = hollowsizex * hollowsizey * hollowsizez; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + float hRadius = _size.X / 2; + float hLength = _size.Z; + + // pi * r2 * h + hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount); + break; + + default: + hollowVolume = 0; + break; + } + volume = volume - hollowVolume; + } + break; + + default: + // we don't have all of the volume formulas yet so + // use the common volume formula for all + volume = _size.X*_size.Y*_size.Z; + break; + } + + // Calculate Path cut effect on volume + // Not exact, in the triangle hollow example + // They should never be zero or less then zero.. + // we'll ignore it if it's less then zero + + // ProfileEnd and ProfileBegin are values + // from 0 to 50000 + + // Turning them back into percentages so that I can cut that percentage off the volume + + float PathCutEndAmount = _pbs.ProfileEnd; + float PathCutStartAmount = _pbs.ProfileBegin; + if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f) + { + float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f); + + // Check the return amount for sanity + if (pathCutAmount >= 0.99f) + pathCutAmount = 0.99f; + + volume = volume - (volume*pathCutAmount); + } + UInt16 taperX = _pbs.PathScaleX; + UInt16 taperY = _pbs.PathScaleY; + float taperFactorX = 0; + float taperFactorY = 0; + + // Mass = density * volume + if (taperX != 100) + { + if (taperX > 100) + { + taperFactorX = 1.0f - ((float)taperX / 200); + //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString()); + } + else + { + taperFactorX = 1.0f - ((100 - (float)taperX) / 100); + //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString()); + } + volume = (float)volume * ((taperFactorX / 3f) + 0.001f); + } + + if (taperY != 100) + { + if (taperY > 100) + { + taperFactorY = 1.0f - ((float)taperY / 200); + //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString()); + } + else + { + taperFactorY = 1.0f - ((100 - (float)taperY) / 100); + //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString()); + } + volume = (float)volume * ((taperFactorY / 3f) + 0.001f); + } + returnMass = m_density*volume; + if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. + + + + // Recursively calculate mass + bool HasChildPrim = false; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + HasChildPrim = true; + } + + } + if (HasChildPrim) + { + OdePrim[] childPrimArr = new OdePrim[0]; + + lock (childrenPrim) + childPrimArr = childrenPrim.ToArray(); + + for (int i = 0; i < childPrimArr.Length; i++) + { + if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) + returnMass += childPrimArr[i].CalculateMass(); + // failsafe, this shouldn't happen but with OpenSim, you never know :) + if (i > 256) + break; + } + } + if (returnMass > _parent_scene.maximumMassObject) + returnMass = _parent_scene.maximumMassObject; + return returnMass; + }// end CalculateMass + + #endregion + + public void setMass() + { + if (Body != (IntPtr) 0) + { + float newmass = CalculateMass(); + + //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString()); + + d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z); + d.BodySetMass(Body, ref pMass); + } + } + + public void disableBody() + { + //this kills the body so things like 'mesh' can re-create it. + lock (this) + { + if (!childPrim) + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + + d.BodyDestroy(Body); + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + prm.Body = IntPtr.Zero; + } + } + } + Body = IntPtr.Zero; + } + } + else + { + _parent_scene.remActivePrim(this); + + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + + Body = IntPtr.Zero; + } + } + m_disabled = true; + m_collisionscore = 0; + } + + private static Dictionary m_MeshToTriMeshMap = new Dictionary(); + + public void setMesh(OdeScene parent_scene, IMesh mesh) + { + // This sleeper is there to moderate how long it takes between + // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object + + //Thread.Sleep(10); + + //Kill Body so that mesh can re-make the geom + if (IsPhysical && Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); + } + } + else + { + disableBody(); + } + } + + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + + mesh.releaseSourceMeshData(); // free up the original mesh data to save memory + if (m_MeshToTriMeshMap.ContainsKey(mesh)) + { + _triMeshData = m_MeshToTriMeshMap[mesh]; + } + else + { + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + m_MeshToTriMeshMap[mesh] = _triMeshData; + } + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + if (prim_geom == IntPtr.Zero) + { + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); + } + } + catch (AccessViolationException) + { + m_log.Error("[PHYSICS]: MESH LOCKED"); + return; + } + + + // if (IsPhysical && Body == (IntPtr) 0) + // { + // Recreate the body + // m_interpenetrationcount = 0; + // m_collisionscore = 0; + + // enableBody(); + // } + } + + public void ProcessTaints(float timestep) + { +//Console.WriteLine("ProcessTaints for " + m_primName ); + if (m_taintadd) + { + changeadd(timestep); + } + + if (prim_geom != IntPtr.Zero) + { + if (!_position.ApproxEquals(m_taintposition, 0f)) + changemove(timestep); + + if (m_taintrot != _orientation) + { + if(childPrim && IsPhysical) // For physical child prim... + { + rotate(timestep); + // KF: ODE will also rotate the parent prim! + // so rotate the root back to where it was + OdePrim parent = (OdePrim)_parent; + parent.rotate(timestep); + } + else + { + //Just rotate the prim + rotate(timestep); + } + } + // + + if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) + changePhysicsStatus(timestep); + // + + if (!_size.ApproxEquals(m_taintsize,0f)) + changesize(timestep); + // + + if (m_taintshape) + changeshape(timestep); + // + + if (m_taintforce) + changeAddForce(timestep); + + if (m_taintaddangularforce) + changeAddAngularForce(timestep); + + if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f)) + changeSetTorque(timestep); + + if (m_taintdisable) + changedisable(timestep); + + if (m_taintselected != m_isSelected) + changeSelectedStatus(timestep); + + if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) + changevelocity(timestep); + + if (m_taintparent != _parent) + changelink(timestep); + + if (m_taintCollidesWater != m_collidesWater) + changefloatonwater(timestep); + + if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) + changeAngularLock(timestep); + + } + else + { + m_log.Error("[PHYSICS]: The scene reused a disposed PhysActor! *waves finger*, Don't be evil. A couple of things can cause this. An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy! An improper buildup (creating the geom failed). Or, the Scene Reused a physics actor after disposing it.)"); + } + } + + + private void changeAngularLock(float timestep) + { + // do we have a Physical object? + if (Body != IntPtr.Zero) + { + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) + { + if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) + { + //d.BodySetFiniteRotationMode(Body, 0); + //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); + createAMotor(m_taintAngularLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = m_taintAngularLock; + + } + + private void changelink(float timestep) + { + // If the newly set parent is not null + // create link + if (_parent == null && m_taintparent != null) + { + if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) + { + OdePrim obj = (OdePrim)m_taintparent; + //obj.disableBody(); +//Console.WriteLine("changelink calls ParentPrim"); + obj.ParentPrim(this); + + /* + if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body) + { + _linkJointGroup = d.JointGroupCreate(0); + m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); + d.JointAttach(m_linkJoint, obj.Body, Body); + d.JointSetFixed(m_linkJoint); + } + */ + } + } + // If the newly set parent is null + // destroy link + else if (_parent != null && m_taintparent == null) + { +//Console.WriteLine(" changelink B"); + + if (_parent is OdePrim) + { + OdePrim obj = (OdePrim)_parent; + obj.ChildDelink(this); + childPrim = false; + //_parent = null; + } + + /* + if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) + d.JointGroupDestroy(_linkJointGroup); + + _linkJointGroup = (IntPtr)0; + m_linkJoint = (IntPtr)0; + */ + } + + _parent = m_taintparent; + m_taintPhysics = m_isphysical; + } + + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) + { +//Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) + { + if (Body == IntPtr.Zero) + { + Body = d.BodyCreate(_parent_scene.world); + setMass(); + } + if (Body != IntPtr.Zero) + { + lock (childrenPrim) + { + if (!childrenPrim.Contains(prim)) + { +//Console.WriteLine("childrenPrim.Add " + prim); + childrenPrim.Add(prim); + + foreach (OdePrim prm in childrenPrim) + { + d.Mass m2; + d.MassSetZero(out m2); + d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z); + + + d.Quaternion quat = new d.Quaternion(); + quat.W = prm._orientation.W; + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + + d.Matrix3 mat = new d.Matrix3(); + d.RfromQ(out mat, ref quat); + d.MassRotate(ref m2, ref mat); + d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z); + d.MassAdd(ref pMass, ref m2); + } + foreach (OdePrim prm in childrenPrim) + { + + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); + continue; + } +//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + + + d.Quaternion quat = new d.Quaternion(); + quat.W = prm._orientation.W; + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + + d.Matrix3 mat = new d.Matrix3(); + d.RfromQ(out mat, ref quat); + if (Body != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, Body); + prm.childPrim = true; + d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); + //d.GeomSetOffsetPosition(prim.prim_geom, + // (Position.X - prm.Position.X) - pMass.c.X, + // (Position.Y - prm.Position.Y) - pMass.c.Y, + // (Position.Z - prm.Position.Z) - pMass.c.Z); + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); + //d.GeomSetOffsetRotation(prm.prim_geom, ref mat); + d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); + d.BodySetMass(Body, ref pMass); + } + else + { + m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body"); + } + + + prm.m_interpenetrationcount = 0; + prm.m_collisionscore = 0; + prm.m_disabled = false; + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) + { + prm.createAMotor(m_angularlock); + } + prm.Body = Body; + _parent_scene.addActivePrim(prm); + } + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + +//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); +//Console.WriteLine(" Post GeomSetCategoryBits 2"); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + + d.Quaternion quat2 = new d.Quaternion(); + quat2.W = _orientation.W; + quat2.X = _orientation.X; + quat2.Y = _orientation.Y; + quat2.Z = _orientation.Z; + + d.Matrix3 mat2 = new d.Matrix3(); + d.RfromQ(out mat2, ref quat2); + d.GeomSetBody(prim_geom, Body); + d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z); + //d.GeomSetOffsetPosition(prim.prim_geom, + // (Position.X - prm.Position.X) - pMass.c.X, + // (Position.Y - prm.Position.Y) - pMass.c.Y, + // (Position.Z - prm.Position.Z) - pMass.c.Z); + //d.GeomSetOffsetRotation(prim_geom, ref mat2); + d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); + d.BodySetMass(Body, ref pMass); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + + + m_interpenetrationcount = 0; + m_collisionscore = 0; + m_disabled = false; + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); + if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene); + _parent_scene.addActivePrim(this); + } + } + } + } + + } + + private void ChildSetGeom(OdePrim odePrim) + { + //if (m_isphysical && Body != IntPtr.Zero) + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + //prm.childPrim = true; + prm.disableBody(); + //prm.m_taintparent = null; + //prm._parent = null; + //prm.m_taintPhysics = false; + //prm.m_disabled = true; + //prm.childPrim = false; + } + } + disableBody(); + + + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + } + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { +//Console.WriteLine("ChildSetGeom calls ParentPrim"); + ParentPrim(prm); + } + } + + } + + private void ChildDelink(OdePrim odePrim) + { + // Okay, we have a delinked child.. need to rebuild the body. + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.childPrim = true; + prm.disableBody(); + //prm.m_taintparent = null; + //prm._parent = null; + //prm.m_taintPhysics = false; + //prm.m_disabled = true; + //prm.childPrim = false; + } + } + disableBody(); + + lock (childrenPrim) + { + //Console.WriteLine("childrenPrim.Remove " + odePrim); + childrenPrim.Remove(odePrim); + } + + + + + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + } + + + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { +//Console.WriteLine("ChildDelink calls ParentPrim"); + ParentPrim(prm); + } + } + + + } + + private void changeSelectedStatus(float timestep) + { + if (m_taintselected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + + // We do the body disable soft twice because 'in theory' a collision could have happened + // in between the disabling and the collision properties setting + // which would wake the physical body up from a soft disabling and potentially cause it to fall + // through the ground. + + // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select + // just one part of the assembly, the rest of the assembly is non-selected and still simulating, + // so that causes the selected part to wake up and continue moving. + + // even if you select all parts of a jointed assembly, it is not guaranteed that the entire + // assembly will stop simulating during the selection, because of the lack of atomicity + // of select operations (their processing could be interrupted by a thread switch, causing + // simulation to continue before all of the selected object notifications trickle down to + // the physics engine). + + // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are + // selected and disabled. then, due to a thread switch, the selection processing is + // interrupted and the physics engine continues to simulate, so the last 50 items, whose + // selection was not yet processed, continues to simulate. this wakes up ALL of the + // first 50 again. then the last 50 are disabled. then the first 50, which were just woken + // up, start simulating again, which in turn wakes up the last 50. + + if (m_isphysical) + { + disableBodySoft(); + } + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + if (m_isphysical) + { + disableBodySoft(); + } + } + else + { + m_collisionCategories = CollisionCategories.Geom; + + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags; + + if (m_collidesLand) + m_collisionFlags |= CollisionCategories.Land; + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + if (m_isphysical) + { + if (Body != IntPtr.Zero) + { + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); + } + } + } + + resetCollisionAccounting(); + m_isSelected = m_taintselected; + }//end changeSelectedStatus + + public void ResetTaints() + { + m_taintposition = _position; + m_taintrot = _orientation; + m_taintPhysics = m_isphysical; + m_taintselected = m_isSelected; + m_taintsize = _size; + m_taintshape = false; + m_taintforce = false; + m_taintdisable = false; + m_taintVelocity = Vector3.Zero; + } + + public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) + { +//Console.WriteLine("CreateGeom:"); + if (_mesh != null) + { + setMesh(_parent_scene, _mesh); + } + else + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) + { + if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) + { + if (((_size.X / 2f) > 0f)) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { +//Console.WriteLine(" CreateGeom 1"); + SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); + ode.dunlock(_parent_scene.world); + return; + } + } + else + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { +//Console.WriteLine(" CreateGeom 2"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); + ode.dunlock(_parent_scene.world); + return; + } + } + } + else + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { +//Console.WriteLine(" CreateGeom 3"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); + ode.dunlock(_parent_scene.world); + return; + } + } + } + + else + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { +//Console.WriteLine(" CreateGeom 4"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); + ode.dunlock(_parent_scene.world); + return; + } + } + } + } + + public void changeadd(float timestep) + { + int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); + + if (targetspace == IntPtr.Zero) + targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); + + m_targetSpace = targetspace; + + if (_mesh == null) + { + if (_parent_scene.needsMeshing(_pbs)) + { + // Don't need to re-enable body.. it's done in SetMesh + _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); + // createmesh returns null when it's a shape that isn't a cube. + // m_log.Debug(m_localID); + } + } + + + lock (_parent_scene.OdeLock) + { +//Console.WriteLine("changeadd 1"); + CreateGeom(m_targetSpace, _mesh); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (m_isphysical && Body == IntPtr.Zero) + { + enableBody(); + } + } + + _parent_scene.geom_name_map[prim_geom] = this.m_primName; + _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; + + changeSelectedStatus(timestep); + + m_taintadd = false; + } + + public void changemove(float timestep) + { + if (m_isphysical) + { + + if (!m_disabled && !m_taintremove && !childPrim) + { + if (Body == IntPtr.Zero) + enableBody(); + //Prim auto disable after 20 frames, + //if you move it, re-enable the prim manually. + if (_parent != null) + { + if (m_linkJoint != IntPtr.Zero) + { + d.JointDestroy(m_linkJoint); + m_linkJoint = IntPtr.Zero; + } + } + if (Body != IntPtr.Zero) + { + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + if (_parent != null) + { + OdePrim odParent = (OdePrim)_parent; + if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) + { +// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? +Console.WriteLine(" JointCreateFixed"); + m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); + d.JointAttach(m_linkJoint, Body, odParent.Body); + d.JointSetFixed(m_linkJoint); + } + } + d.BodyEnable(Body); + if (m_vehicle.Type != Vehicle.TYPE_NONE) + { + m_vehicle.Enable(Body, _parent_scene); + } + } + else + { + m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario."); + } + } + //else + // { + //m_log.Debug("[BUG]: race!"); + //} + } + else + { + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + _parent_scene.waitForSpaceUnlock(m_targetSpace); + + IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); + m_targetSpace = tempspace; + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + d.SpaceAdd(m_targetSpace, prim_geom); + } + } + + changeSelectedStatus(timestep); + + resetCollisionAccounting(); + m_taintposition = _position; + } + + public void Move(float timestep) + { + float fx = 0; + float fy = 0; + float fz = 0; + + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; + + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. + { +//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type + + // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); + if (m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(timestep, _parent_scene); + } + else + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 + // NON-'VEHICLES' are dealt with here + if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) + { + d.Vector3 avel2 = d.BodyGetAngularVel(Body); + if (m_angularlock.X == 1) + avel2.X = 0; + if (m_angularlock.Y == 1) + avel2.Y = 0; + if (m_angularlock.Z == 1) + avel2.Z = 0; + d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); + } + //float PID_P = 900.0f; + + float m_mass = CalculateMass(); + +// fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + + + //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // NB Prims in ODE are no subject to global gravity + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass + + if (m_usePID) + { +//if(frcount == 0) Console.WriteLine("PID " + m_primName); + // KF - this is for object MoveToTarget. + + //if (!d.BodyIsEnabled(Body)) + //d.BodySetForce(Body, 0f, 0f, 0f); + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + //PidStatus = true; + + // PhysicsVector vec = new PhysicsVector(); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + if (m_useHoverPID && !m_usePID) + { +//Console.WriteLine("Hover " + m_primName); + + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + d.BodyAddForce(Body, 0, 0, fz); + //KF this prevents furthur motions return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end m_useHoverPID && !m_usePID + + if (m_useAPID) + { + // RotLookAt, apparently overrides all other rotation sources. Inputs: + // Quaternion m_APIDTarget + // float m_APIDStrength // From SL experiments, this is the time to get there + // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly + // Also in SL the mass of the object has no effect on time to get there. + // Factors: +//if(frcount == 0) Console.WriteLine("APID "); + // get present body rotation + float limit = 1.0f; + float scaler = 50f; // adjusts damping time + float RLAservo = 0f; + + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; + float diff_angle; + Vector3 diff_axis; + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); + diff_axis.Normalize(); + if(diff_angle > 0.01f) // diff_angle is always +ve + { +// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if(diff_angle > limit) diff_angle = limit; // cap the rotate rate +// RLAservo = timestep / m_APIDStrength * m_mass * scaler; + // rotforce = rotforce * RLAservo * diff_angle ; + // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); + RLAservo = timestep / m_APIDStrength * scaler; + rotforce = rotforce * RLAservo * diff_angle ; + d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); +//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } +//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); + } // end m_useAPID + + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; + + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + //m_taintdisable = true; + //base.RaiseOutOfBounds(Position); + //d.BodySetLinearVel(Body, fx, fy, 0f); + if (!d.BodyIsEnabled(Body)) + { + // A physical body at rest on a surface will auto-disable after a while, + // this appears to re-enable it incase the surface it is upon vanishes, + // and the body should fall again. + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); + } + + // 35x10 = 350n times the mass per second applied maximum. + float nmax = 35f * m_mass; + float nmin = -35f * m_mass; + + + if (fx > nmax) + fx = nmax; + if (fx < nmin) + fx = nmin; + if (fy > nmax) + fy = nmax; + if (fy < nmin) + fy = nmin; + d.BodyAddForce(Body, fx, fy, fz); +//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + } + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; +//Console.WriteLine("Nothing " + m_primName); + + } + } + + + + public void rotate(float timestep) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + if (Body != IntPtr.Zero) + { + // KF: If this is a root prim do BodySet + d.BodySetQuaternion(Body, ref myrot); + if (m_isphysical) + { + if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + } + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + resetCollisionAccounting(); + m_taintrot = _orientation; + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_disabled = false; + } + + public void changedisable(float timestep) + { + m_disabled = true; + if (Body != IntPtr.Zero) + { + d.BodyDisable(Body); + Body = IntPtr.Zero; + } + + m_taintdisable = false; + } + + public void changePhysicsStatus(float timestep) + { + if (m_isphysical == true) + { + if (Body == IntPtr.Zero) + { + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeshape(2f); + } + else + { + enableBody(); + } + } + } + else + { + if (Body != IntPtr.Zero) + { + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + + + if (prim_geom != IntPtr.Zero) + { + try + { + d.GeomDestroy(prim_geom); + prim_geom = IntPtr.Zero; + _mesh = null; + } + catch (System.AccessViolationException) + { + prim_geom = IntPtr.Zero; + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + } +//Console.WriteLine("changePhysicsStatus for " + m_primName ); + changeadd(2f); + } + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); + } + } + else + { + disableBody(); + } + } + } + + changeSelectedStatus(timestep); + + resetCollisionAccounting(); + m_taintPhysics = m_isphysical; + } + + public void changesize(float timestamp) + { + + string oldname = _parent_scene.geom_name_map[prim_geom]; + + if (_size.X <= 0) _size.X = 0.01f; + if (_size.Y <= 0) _size.Y = 0.01f; + if (_size.Z <= 0) _size.Z = 0.01f; + + // Cleanup of old prim geometry + if (_mesh != null) + { + // Cleanup meshing here + } + //kill body to rebuild + if (IsPhysical && Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); + } + } + else + { + disableBody(); + } + } + if (d.SpaceQuery(m_targetSpace, prim_geom)) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + d.SpaceRemove(m_targetSpace, prim_geom); + } + d.GeomDestroy(prim_geom); + prim_geom = IntPtr.Zero; + // we don't need to do space calculation because the client sends a position update also. + + // Construction of new prim + if (_parent_scene.needsMeshing(_pbs)) + { + float meshlod = _parent_scene.meshSculptLOD; + + if (IsPhysical) + meshlod = _parent_scene.MeshSculptphysicalLOD; + // Don't need to re-enable body.. it's done in SetMesh + + IMesh mesh = null; + + if (_parent_scene.needsMeshing(_pbs)) + mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + + //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); +//Console.WriteLine("changesize 1"); + CreateGeom(m_targetSpace, mesh); + + + } + else + { + _mesh = null; +//Console.WriteLine("changesize 2"); + CreateGeom(m_targetSpace, _mesh); + } + + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + + //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); + if (IsPhysical && Body == IntPtr.Zero && !childPrim) + { + // Re creates body on size. + // EnableBody also does setMass() + enableBody(); + d.BodyEnable(Body); + } + + _parent_scene.geom_name_map[prim_geom] = oldname; + + changeSelectedStatus(timestamp); + if (childPrim) + { + if (_parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildSetGeom(this); + } + } + resetCollisionAccounting(); + m_taintsize = _size; + } + + + + public void changefloatonwater(float timestep) + { + m_collidesWater = m_taintCollidesWater; + + if (prim_geom != IntPtr.Zero) + { + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } + else + { + m_collisionFlags &= ~CollisionCategories.Water; + } + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } + + public void changeshape(float timestamp) + { + string oldname = _parent_scene.geom_name_map[prim_geom]; + + // Cleanup of old prim geometry and Bodies + if (IsPhysical && Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); + } + } + else + { + disableBody(); + } + } + try + { + d.GeomDestroy(prim_geom); + } + catch (System.AccessViolationException) + { + prim_geom = IntPtr.Zero; + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + prim_geom = IntPtr.Zero; + // we don't need to do space calculation because the client sends a position update also. + if (_size.X <= 0) _size.X = 0.01f; + if (_size.Y <= 0) _size.Y = 0.01f; + if (_size.Z <= 0) _size.Z = 0.01f; + // Construction of new prim + + if (_parent_scene.needsMeshing(_pbs)) + { + // Don't need to re-enable body.. it's done in SetMesh + float meshlod = _parent_scene.meshSculptLOD; + + if (IsPhysical) + meshlod = _parent_scene.MeshSculptphysicalLOD; + + IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + // createmesh returns null when it doesn't mesh. + CreateGeom(m_targetSpace, mesh); + } + else + { + _mesh = null; +//Console.WriteLine("changeshape"); + CreateGeom(m_targetSpace, null); + } + + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + //myrot.W = _orientation.w; + myrot.W = _orientation.W; + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + d.GeomSetQuaternion(prim_geom, ref myrot); + + //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); + if (IsPhysical && Body == IntPtr.Zero) + { + // Re creates body on size. + // EnableBody also does setMass() + enableBody(); + if (Body != IntPtr.Zero) + { + d.BodyEnable(Body); + } + } + _parent_scene.geom_name_map[prim_geom] = oldname; + + changeSelectedStatus(timestamp); + if (childPrim) + { + if (_parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildSetGeom(this); + } + } + resetCollisionAccounting(); + m_taintshape = false; + } + + public void changeAddForce(float timestamp) + { + if (!m_isSelected) + { + lock (m_forcelist) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (IsPhysical) + { + Vector3 iforce = Vector3.Zero; + int i = 0; + try + { + for (i = 0; i < m_forcelist.Count; i++) + { + + iforce = iforce + (m_forcelist[i] * 100); + } + } + catch (IndexOutOfRangeException) + { + m_forcelist = new List(); + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_taintforce = false; + return; + } + catch (ArgumentOutOfRangeException) + { + m_forcelist = new List(); + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_taintforce = false; + return; + } + d.BodyEnable(Body); + d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); + } + m_forcelist.Clear(); + } + + m_collisionscore = 0; + m_interpenetrationcount = 0; + } + + m_taintforce = false; + + } + + + + public void changeSetTorque(float timestamp) + { + if (!m_isSelected) + { + if (IsPhysical && Body != IntPtr.Zero) + { + d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); + } + } + + m_taintTorque = Vector3.Zero; + } + + public void changeAddAngularForce(float timestamp) + { + if (!m_isSelected) + { + lock (m_angularforcelist) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (IsPhysical) + { + Vector3 iforce = Vector3.Zero; + for (int i = 0; i < m_angularforcelist.Count; i++) + { + iforce = iforce + (m_angularforcelist[i] * 100); + } + d.BodyEnable(Body); + d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); + + } + m_angularforcelist.Clear(); + } + + m_collisionscore = 0; + m_interpenetrationcount = 0; + } + + m_taintaddangularforce = false; + } + + private void changevelocity(float timestep) + { + if (!m_isSelected) + { + Thread.Sleep(20); + if (IsPhysical) + { + if (Body != IntPtr.Zero) + { + d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); + } + } + + //resetCollisionAccounting(); + } + m_taintVelocity = Vector3.Zero; + } + + public override bool IsPhysical + { + get { return m_isphysical; } + set { + m_isphysical = value; + if (!m_isphysical) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + } + } + + public void setPrimForRemoval() + { + m_taintremove = true; + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return iscolliding; } + set { iscolliding = value; } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get { return _position; } + + set { _position = value; + //m_log.Info("[PHYSICS]: " + _position.ToString()); + } + } + + public override Vector3 Size + { + get { return _size; } + set + { + if (value.IsFinite()) + { + _size = value; + } + else + { + m_log.Warn("[PHYSICS]: Got NaN Size on object"); + } + } + } + + public override float Mass + { + get { return CalculateMass(); } + } + + public override Vector3 Force + { + //get { return Vector3.Zero; } + get { return m_force; } + set + { + if (value.IsFinite()) + { + m_force = value; + } + else + { + m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object"); + } + } + } + + public override int VehicleType + { + get { return (int)m_vehicle.Type; } + set { m_vehicle.ProcessTypeChange((Vehicle)value); } + } + + public override void VehicleFloatParam(int param, float value) + { + m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value); + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); + } + + public override void SetVolumeDetect(int param) + { + lock (_parent_scene.OdeLock) + { + m_isVolumeDetect = (param!=0); + } + } + + public override Vector3 CenterOfMass + { + get { return Vector3.Zero; } + } + + public override Vector3 GeometricCenter + { + get { return Vector3.Zero; } + } + + public override PrimitiveBaseShape Shape + { + set + { + _pbs = value; + m_taintshape = true; + } + } + + public override Vector3 Velocity + { + get + { + // Averate previous velocity with the new one so + // client object interpolation works a 'little' better + if (_zeroFlag) + return Vector3.Zero; + + Vector3 returnVelocity = Vector3.Zero; + returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; + return returnVelocity; + } + set + { + if (value.IsFinite()) + { + _velocity = value; + + m_taintVelocity = value; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got NaN Velocity in Object"); + } + + } + } + + public override Vector3 Torque + { + get + { + if (!m_isphysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return _torque; + } + + set + { + if (value.IsFinite()) + { + m_taintTorque = value; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got NaN Torque in Object"); + } + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return _orientation; } + set + { + if (QuaternionIsFinite(value)) + { + _orientation = value; + } + else + m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); + + } + } + + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + } + + + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + lock (m_forcelist) + m_forcelist.Add(force); + + m_taintforce = true; + } + else + { + m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object"); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + m_angularforcelist.Add(force); + m_taintaddangularforce = true; + } + else + { + m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object"); + } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + } + else + { + m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object"); + } + } + } + + public override void CrossingFailure() + { + m_crossingfailures++; + if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + base.RaiseOutOfBounds(_position); + return; + } + else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); + } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override void link(PhysicsActor obj) + { + m_taintparent = obj; + } + + public override void delink() + { + m_taintparent = null; + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + m_taintAngularLock = axis; + } + else + { + m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object"); + } + } + + public void UpdatePositionAndVelocity() + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null) + { + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + if (Body != (IntPtr)0) // FIXME -> or if it is a joint + { + d.Vector3 vec = d.BodyGetPosition(Body); + d.Quaternion ori = d.BodyGetQuaternion(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + d.Vector3 torque = d.BodyGetTorque(Body); + _torque = new Vector3(torque.X, torque.Y, torque.Z); + Vector3 l_position = Vector3.Zero; + Quaternion l_orientation = Quaternion.Identity; + + // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) + //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + + m_lastposition = _position; + m_lastorientation = _orientation; + + l_position.X = vec.X; + l_position.Y = vec.Y; + l_position.Z = vec.Z; + l_orientation.X = ori.X; + l_orientation.Y = ori.Y; + l_orientation.Z = ori.Z; + l_orientation.W = ori.W; + +// if(l_position.Y != m_lastposition.Y){ +// Console.WriteLine("UP&V {0} {1}", m_primName, l_position); +// } + + if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) + { + //base.RaiseOutOfBounds(l_position); + + if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + _position = l_position; + //_parent_scene.remActivePrim(this); + if (_parent == null) + base.RequestPhysicsterseUpdate(); + return; + } + else + { + if (_parent == null) + base.RaiseOutOfBounds(l_position); + return; + } + } + + if (l_position.Z < 0) + { + // This is so prim that get lost underground don't fall forever and suck up + // + // Sim resources and memory. + // Disables the prim's movement physics.... + // It's a hack and will generate a console message if it fails. + + //IsPhysical = false; + if (_parent == null) + base.RaiseOutOfBounds(_position); + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + if (_parent == null) + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + //outofBounds = true; + } + + //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); +//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); + if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) +// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) + && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large + { + _zeroFlag = true; +//Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } + + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + //_orientation.w = 0f; + //_orientation.X = 0f; + //_orientation.Y = 0f; + //_orientation.Z = 0f; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } + } + + m_lastVelocity = _velocity; + + _position = l_position; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _acceleration = ((_velocity - m_lastVelocity) / 0.1f); + _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); + //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); + + if (_velocity.ApproxEquals(pv, 0.5f)) + { + m_rotationalVelocity = pv; + } + else + { + m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); + } + + //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } + } + else + { + throttleCounter++; + } + } + m_lastposition = l_position; + } + else + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + } + } + } + + public override bool FloatOnWater + { + set { + m_taintCollidesWater = value; + _parent_scene.AddPhysicsActorTaint(this); + } + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); + } + } + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + // For RotLookAt + public override Quaternion APIDTarget { set { m_APIDTarget = value; } } + public override bool APIDActive { set { m_useAPID = value; } } + public override float APIDStrength { set { m_APIDStrength = value; } } + public override float APIDDamping { set { m_APIDDamping = value; } } + + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + float axisnum = 3; + + axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); + + // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); + + + // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. + d.Mass objMass; + d.MassSetZero(out objMass); + DMassCopy(ref pMass, ref objMass); + + //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); + + Matrix4 dMassMat = FromDMass(objMass); + + Matrix4 mathmat = Inverse(dMassMat); + + /* + //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]); + + mathmat = Inverse(mathmat); + + + objMass = FromMatrix4(mathmat, ref objMass); + //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); + + mathmat = Inverse(mathmat); + */ + if (axis.X == 0) + { + mathmat.M33 = 50.0000001f; + //objMass.I.M22 = 0; + } + if (axis.Y == 0) + { + mathmat.M22 = 50.0000001f; + //objMass.I.M11 = 0; + } + if (axis.Z == 0) + { + mathmat.M11 = 50.0000001f; + //objMass.I.M00 = 0; + } + + + + mathmat = Inverse(mathmat); + objMass = FromMatrix4(mathmat, ref objMass); + //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); + + //return; + if (d.MassCheck(ref objMass)) + { + d.BodySetMass(Body, ref objMass); + } + else + { + //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); + } + + if (axisnum <= 0) + return; + // int dAMotorEuler = 1; + + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + d.JointSetAMotorMode(Amotor, 0); + + d.JointSetAMotorNumAxes(Amotor,(int)axisnum); + int i = 0; + + if (axis.X == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); + i++; + } + + if (axis.Y == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); + i++; + } + + if (axis.Z == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); + i++; + } + + for (int j = 0; j < (int)axisnum; j++) + { + //d.JointSetAMotorAngle(Amotor, j, 0); + } + + //d.JointSetAMotorAngle(Amotor, 1, 0); + //d.JointSetAMotorAngle(Amotor, 2, 0); + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); + //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f); + d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// + + } + + public Matrix4 FromDMass(d.Mass pMass) + { + Matrix4 obj; + obj.M11 = pMass.I.M00; + obj.M12 = pMass.I.M01; + obj.M13 = pMass.I.M02; + obj.M14 = 0; + obj.M21 = pMass.I.M10; + obj.M22 = pMass.I.M11; + obj.M23 = pMass.I.M12; + obj.M24 = 0; + obj.M31 = pMass.I.M20; + obj.M32 = pMass.I.M21; + obj.M33 = pMass.I.M22; + obj.M34 = 0; + obj.M41 = 0; + obj.M42 = 0; + obj.M43 = 0; + obj.M44 = 1; + return obj; + } + + public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) + { + obj.I.M00 = pMat[0, 0]; + obj.I.M01 = pMat[0, 1]; + obj.I.M02 = pMat[0, 2]; + obj.I.M10 = pMat[1, 0]; + obj.I.M11 = pMat[1, 1]; + obj.I.M12 = pMat[1, 2]; + obj.I.M20 = pMat[2, 0]; + obj.I.M21 = pMat[2, 1]; + obj.I.M22 = pMat[2, 2]; + return obj; + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.addCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + _parent_scene.remCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + CollisionEventsThisFrame.addCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) + CollisionEventsThisFrame = null; + else + CollisionEventsThisFrame = new CollisionEventUpdate(); + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + public static Matrix4 Inverse(Matrix4 pMat) + { + if (determinant3x3(pMat) == 0) + { + return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible + } + + + + return (Adjoint(pMat) / determinant3x3(pMat)); + } + + public static Matrix4 Adjoint(Matrix4 pMat) + { + Matrix4 adjointMatrix = new Matrix4(); + for (int i=0; i<4; i++) + { + for (int j=0; j<4; j++) + { + Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); + } + } + + adjointMatrix = Transpose(adjointMatrix); + return adjointMatrix; + } + + public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) + { + Matrix4 minor = new Matrix4(); + int m = 0, n = 0; + for (int i = 0; i < 4; i++) + { + if (i == iRow) + continue; + n = 0; + for (int j = 0; j < 4; j++) + { + if (j == iCol) + continue; + Matrix4SetValue(ref minor, m,n, matrix[i, j]); + n++; + } + m++; + } + return minor; + } + + public static Matrix4 Transpose(Matrix4 pMat) + { + Matrix4 transposeMatrix = new Matrix4(); + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); + return transposeMatrix; + } + + public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) + { + switch (r) + { + case 0: + switch (c) + { + case 0: + pMat.M11 = val; + break; + case 1: + pMat.M12 = val; + break; + case 2: + pMat.M13 = val; + break; + case 3: + pMat.M14 = val; + break; + } + + break; + case 1: + switch (c) + { + case 0: + pMat.M21 = val; + break; + case 1: + pMat.M22 = val; + break; + case 2: + pMat.M23 = val; + break; + case 3: + pMat.M24 = val; + break; + } + + break; + case 2: + switch (c) + { + case 0: + pMat.M31 = val; + break; + case 1: + pMat.M32 = val; + break; + case 2: + pMat.M33 = val; + break; + case 3: + pMat.M34 = val; + break; + } + + break; + case 3: + switch (c) + { + case 0: + pMat.M41 = val; + break; + case 1: + pMat.M42 = val; + break; + case 2: + pMat.M43 = val; + break; + case 3: + pMat.M44 = val; + break; + } + + break; + } + } + private static float determinant3x3(Matrix4 pMat) + { + float det = 0; + float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; + float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; + float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; + float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; + float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; + float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; + + det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); + return det; + + } + + private static void DMassCopy(ref d.Mass src, ref d.Mass dst) + { + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + } + + } +} diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs new file mode 100644 index 0000000..7314107 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs @@ -0,0 +1,375 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenMetaverse; +using OpenSim.Region.Physics.Manager; +using Ode.NET; +using log4net; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Processes raycast requests as ODE is in a state to be able to do them. + /// This ensures that it's thread safe and there will be no conflicts. + /// Requests get returned by a different thread then they were requested by. + /// + public class ODERayCastRequestManager + { + /// + /// Pending Raycast Requests + /// + protected List m_PendingRequests = new List(); + + /// + /// Scene that created this object. + /// + private OdeScene m_scene; + + /// + /// ODE contact array to be filled by the collision testing + /// + d.ContactGeom[] contacts = new d.ContactGeom[5]; + + /// + /// ODE near callback delegate + /// + private d.NearCallback nearCallback; + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + private List m_contactResults = new List(); + + + public ODERayCastRequestManager(OdeScene pScene) + { + m_scene = pScene; + nearCallback = near; + + } + + /// + /// Queues a raycast + /// + /// Origin of Ray + /// Ray normal + /// Ray length + /// Return method to send the results + public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + lock (m_PendingRequests) + { + ODERayCastRequest req = new ODERayCastRequest(); + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + + m_PendingRequests.Add(req); + } + } + + /// + /// Process all queued raycast requests + /// + /// Time in MS the raycasts took to process. + public int ProcessQueuedRequests() + { + int time = System.Environment.TickCount; + lock (m_PendingRequests) + { + if (m_PendingRequests.Count > 0) + { + ODERayCastRequest[] reqs = m_PendingRequests.ToArray(); + for (int i = 0; i < reqs.Length; i++) + { + if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast + RayCast(reqs[i]); // if there isn't anyone to send results + } + /* + foreach (ODERayCastRequest req in m_PendingRequests) + { + if (req.callbackMethod != null) // quick optimization here, don't raycast + RayCast(req); // if there isn't anyone to send results to + + } + */ + m_PendingRequests.Clear(); + } + } + + lock (m_contactResults) + m_contactResults.Clear(); + + return System.Environment.TickCount - time; + } + + /// + /// Method that actually initiates the raycast + /// + /// + private void RayCast(ODERayCastRequest req) + { + // Create the ray + IntPtr ray = d.CreateRay(m_scene.space, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + + // Collide test + d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback); + + // Remove Ray + d.GeomDestroy(ray); + + + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = 999999999999f; + Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + hitYN = true; + snormal = cResult.Normal; + } + } + + m_contactResults.Clear(); + } + + // Return results + if (req.callbackMethod != null) + req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + + // This is the standard Near. Uses space AABBs to speed up detection. + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + + //Don't test against heightfield Geom, or you'll be sorry! + + /* + terminate called after throwing an instance of 'std::bad_alloc' + what(): std::bad_alloc + Stacktrace: + + at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004> + at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff> + at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280> + at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff + fffff> + at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004> + at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff> + at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) < + 0x00114> + at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb> + at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6> + at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042> + at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e> + at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019> + at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff> + + Native stacktrace: + + mono [0x80d2a42] + [0xb7f5840c] + /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018] + /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988] + /usr/lib/libstdc++.so.6 [0xb45fa865] + /usr/lib/libstdc++.so.6 [0xb45fa8a2] + /usr/lib/libstdc++.so.6 [0xb45fa9da] + /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033] + /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d] + libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4] + libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b] + libode.so(dCollide+0x102) [0xb46571b2] + [0x95cfdec9] + [0x8ea07fe1] + [0xab260146] + libode.so [0xb465a5c4] + libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5] + libode.so(dSpaceCollide2+0x177) [0xb465ac67] + [0x95cf978e] + [0x8ea07945] + [0x95cf2bbc] + [0xab2787e7] + [0xab419fb3] + [0xab416657] + [0xab415bda] + [0xb609b08e] + mono(mono_runtime_delegate_invoke+0x34) [0x8192534] + mono [0x81a2f0f] + mono [0x81d28b6] + mono [0x81ea2c6] + /lib/i686/cmov/libpthread.so.0 [0xb7e744c0] + /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de] + */ + + // Exclude heightfield geom + + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass) + return; + + // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms. + if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) + { + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + // Separating static prim geometry spaces. + // We'll be calling near recursivly if one + // of them is a space to find all of the + // contact points in the space + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide test a space"); + return; + } + //Colliding a space or a geom with a space or a geom. so drill down + + //Collide all geoms in each space.. + //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback); + //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback); + return; + } + + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + int count = 0; + try + { + + if (g1 == g2) + return; // Can't collide with yourself + + lock (contacts) + { + count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf); + } + } + 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."); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); + return; + } + + PhysicsActor p1 = null; + PhysicsActor p2 = null; + + if (g1 != IntPtr.Zero) + m_scene.actor_name_map.TryGetValue(g1, out p1); + + if (g2 != IntPtr.Zero) + m_scene.actor_name_map.TryGetValue(g1, out p2); + + // Loop over contacts, build results. + for (int i = 0; i < count; i++) + { + if (p1 != null) { + if (p1 is OdePrim) + { + ContactResult collisionresult = new ContactResult(); + + collisionresult.ConsumerID = ((OdePrim)p1).m_localID; + collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z); + collisionresult.Depth = contacts[i].depth; + collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y, + contacts[i].normal.Z); + lock (m_contactResults) + m_contactResults.Add(collisionresult); + } + } + + if (p2 != null) + { + if (p2 is OdePrim) + { + ContactResult collisionresult = new ContactResult(); + + collisionresult.ConsumerID = ((OdePrim)p2).m_localID; + collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z); + collisionresult.Depth = contacts[i].depth; + collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y, + contacts[i].normal.Z); + + lock (m_contactResults) + m_contactResults.Add(collisionresult); + } + } + + + } + + } + + /// + /// Dereference the creator scene so that it can be garbage collected if needed. + /// + internal void Dispose() + { + m_scene = null; + } + } + + public struct ODERayCastRequest + { + public Vector3 Origin; + public Vector3 Normal; + public float length; + public RaycastCallback callbackMethod; + } + + public struct ContactResult + { + public Vector3 Pos; + public float Depth; + public uint ConsumerID; + public Vector3 Normal; + } +} diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs new file mode 100644 index 0000000..b4a3c48 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs @@ -0,0 +1,48 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using OpenMetaverse; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenSim.Region.Physics.OdePlugin; + +namespace OpenSim.Region.Physics.OdePlugin +{ + class OdePhysicsJoint : PhysicsJoint + { + public override bool IsInPhysicsEngine + { + get + { + return (jointID != IntPtr.Zero); + } + } + public IntPtr jointID; + } +} diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs new file mode 100644 index 0000000..f48649e --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -0,0 +1,3865 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +//#define USE_DRAWSTUFF + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using Ode.NET; +#if USE_DRAWSTUFF +using Drawstuff.NET; +#endif +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; + +//using OpenSim.Region.Physics.OdePlugin.Meshing; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// ODE plugin + /// + public class OdePlugin : IPhysicsPlugin + { + //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); + + private CollisionLocker ode; + private OdeScene _mScene; + + public OdePlugin() + { + ode = new CollisionLocker(); + } + + public bool Init() + { + return true; + } + + public PhysicsScene GetScene(String sceneIdentifier) + { + if (_mScene == null) + { + // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to + // http://opensimulator.org/mantis/view.php?id=2750). + d.InitODE(); + + _mScene = new OdeScene(ode, sceneIdentifier); + } + return (_mScene); + } + + public string GetName() + { + return ("ChODE"); + } + + public void Dispose() + { + } + } + + public enum StatusIndicators : int + { + Generic = 0, + Start = 1, + End = 2 + } + + public struct sCollisionData + { + public uint ColliderLocalId; + public uint CollidedWithLocalId; + public int NumberOfCollisions; + public int CollisionType; + public int StatusIndicator; + public int lastframe; + } + + [Flags] + public enum CollisionCategories : int + { + Disabled = 0, + Geom = 0x00000001, + Body = 0x00000002, + Space = 0x00000004, + Character = 0x00000008, + Land = 0x00000010, + Water = 0x00000020, + Wind = 0x00000040, + Sensor = 0x00000080, + Selected = 0x00000100 + } + + /// + /// Material type for a primitive + /// + public enum Material : int + { + /// + Stone = 0, + /// + Metal = 1, + /// + Glass = 2, + /// + Wood = 3, + /// + Flesh = 4, + /// + Plastic = 5, + /// + Rubber = 6 + + } + + public sealed class OdeScene : PhysicsScene + { + private readonly ILog m_log; + // private Dictionary m_storedCollisions = new Dictionary(); + + CollisionLocker ode; + + private Random fluidRandomizer = new Random(Environment.TickCount); + + private const uint m_regionWidth = Constants.RegionSize; + private const uint m_regionHeight = Constants.RegionSize; + + private float ODE_STEPSIZE = 0.020f; + private float metersInSpace = 29.9f; + private float m_timeDilation = 1.0f; + + public float gravityx = 0f; + public float gravityy = 0f; + public float gravityz = -9.8f; + + private float contactsurfacelayer = 0.001f; + + private int worldHashspaceLow = -4; + private int worldHashspaceHigh = 128; + + private int smallHashspaceLow = -4; + private int smallHashspaceHigh = 66; + + private float waterlevel = 0f; + private int framecount = 0; + //private int m_returncollisions = 10; + + private readonly IntPtr contactgroup; + + internal IntPtr LandGeom; + internal IntPtr WaterGeom; + + private float nmTerrainContactFriction = 255.0f; + private float nmTerrainContactBounce = 0.1f; + private float nmTerrainContactERP = 0.1025f; + + private float mTerrainContactFriction = 75f; + private float mTerrainContactBounce = 0.1f; + private float mTerrainContactERP = 0.05025f; + + private float nmAvatarObjectContactFriction = 250f; + private float nmAvatarObjectContactBounce = 0.1f; + + private float mAvatarObjectContactFriction = 75f; + private float mAvatarObjectContactBounce = 0.1f; + + private float avPIDD = 3200f; + private float avPIDP = 1400f; + private float avCapRadius = 0.37f; + private float avStandupTensor = 2000000f; + private bool avCapsuleTilted = true; // true = old compatibility mode with leaning capsule; false = new corrected mode + public bool IsAvCapsuleTilted { get { return avCapsuleTilted; } set { avCapsuleTilted = value; } } + private float avDensity = 80f; + private float avHeightFudgeFactor = 0.52f; + private float avMovementDivisorWalk = 1.3f; + private float avMovementDivisorRun = 0.8f; + private float minimumGroundFlightOffset = 3f; + public float maximumMassObject = 10000.01f; + + public bool meshSculptedPrim = true; + public bool forceSimplePrimMeshing = false; + + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 16; + + public float geomDefaultDensity = 10.000006836f; + + public int geomContactPointsStartthrottle = 3; + public int geomUpdatesPerThrottledUpdate = 15; + + public float bodyPIDD = 35f; + public float bodyPIDG = 25; + + public int geomCrossingFailuresBeforeOutofbounds = 5; + + public float bodyMotorJointMaxforceTensor = 2; + + public int bodyFramesAutoDisable = 20; + + + + private float[] _watermap; + private bool m_filterCollisions = true; + + private d.NearCallback nearCallback; + public d.TriCallback triCallback; + public d.TriArrayCallback triArrayCallback; + private readonly HashSet _characters = new HashSet(); + private readonly HashSet _prims = new HashSet(); + private readonly HashSet _activeprims = new HashSet(); + private readonly HashSet _taintedPrimH = new HashSet(); + private readonly Object _taintedPrimLock = new Object(); + private readonly List _taintedPrimL = new List(); + private readonly HashSet _taintedActors = new HashSet(); + private readonly List _perloopContact = new List(); + private readonly List _collisionEventPrim = new List(); + private readonly HashSet _badCharacter = new HashSet(); + public Dictionary geom_name_map = new Dictionary(); + public Dictionary actor_name_map = new Dictionary(); + private bool m_NINJA_physics_joints_enabled = false; + //private Dictionary jointpart_name_map = new Dictionary(); + private readonly Dictionary> joints_connecting_actor = new Dictionary>(); + private d.ContactGeom[] contacts; + private readonly List requestedJointsToBeCreated = new List(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active + private readonly List pendingJoints = new List(); // can lock for longer. accessed only by OdeScene. + private readonly List activeJoints = new List(); // can lock for longer. accessed only by OdeScene. + private readonly List requestedJointsToBeDeleted = new List(); // lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active + private Object externalJointRequestsLock = new Object(); + private readonly Dictionary SOPName_to_activeJoint = new Dictionary(); + private readonly Dictionary SOPName_to_pendingJoint = new Dictionary(); + private readonly DoubleDictionary RegionTerrain = new DoubleDictionary(); + private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); + + private d.Contact contact; + private d.Contact TerrainContact; + private d.Contact AvatarMovementprimContact; + private d.Contact AvatarMovementTerrainContact; + private d.Contact WaterContact; + private d.Contact[,] m_materialContacts; + +//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it +//Ckrinke private int m_randomizeWater = 200; + private int m_physicsiterations = 10; + private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag + private readonly PhysicsActor PANull = new NullPhysicsActor(); + private float step_time = 0.0f; +//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it +//Ckrinke private int ms = 0; + public IntPtr world; + //private bool returncollisions = false; + // private uint obj1LocalID = 0; + private uint obj2LocalID = 0; + //private int ctype = 0; + private OdeCharacter cc1; + private OdePrim cp1; + private OdeCharacter cc2; + private OdePrim cp2; + //private int cStartStop = 0; + //private string cDictKey = ""; + + public IntPtr space; + + //private IntPtr tmpSpace; + // split static geometry collision handling into spaces of 30 meters + public IntPtr[,] staticPrimspace; + + public Object OdeLock; + + public IMesher mesher; + + private IConfigSource m_config; + + public bool physics_logging = false; + public int physics_logging_interval = 0; + public bool physics_logging_append_existing_logfile = false; + + public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f); + public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f); + + // TODO: unused: private uint heightmapWidth = m_regionWidth + 1; + // TODO: unused: private uint heightmapHeight = m_regionHeight + 1; + // TODO: unused: private uint heightmapWidthSamples; + // TODO: unused: private uint heightmapHeightSamples; + + private volatile int m_global_contactcount = 0; + + private Vector3 m_worldOffset = Vector3.Zero; + public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + private PhysicsScene m_parentScene = null; + + private ODERayCastRequestManager m_rayCastManager; + + /// + /// Initiailizes the scene + /// Sets many properties that ODE requires to be stable + /// These settings need to be tweaked 'exactly' right or weird stuff happens. + /// + public OdeScene(CollisionLocker dode, string sceneIdentifier) + { + m_log + = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); + + OdeLock = new Object(); + ode = dode; + nearCallback = near; + triCallback = TriCallback; + triArrayCallback = TriArrayCallback; + m_rayCastManager = new ODERayCastRequestManager(this); + lock (OdeLock) + { + // Create the world and the first space + world = d.WorldCreate(); + space = d.HashSpaceCreate(IntPtr.Zero); + + + contactgroup = d.JointGroupCreate(0); + //contactgroup + + d.WorldSetAutoDisableFlag(world, false); + #if USE_DRAWSTUFF + + Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); + viewthread.Start(); + #endif + } + + + _watermap = new float[258 * 258]; + + // Zero out the prim spaces array (we split our space into smaller spaces so + // we can hit test less. + } + +#if USE_DRAWSTUFF + public void startvisualization(object o) + { + ds.Functions fn; + fn.version = ds.VERSION; + fn.start = new ds.CallbackFunction(start); + fn.step = new ds.CallbackFunction(step); + fn.command = new ds.CallbackFunction(command); + fn.stop = null; + fn.path_to_textures = "./textures"; + string[] args = new string[0]; + ds.SimulationLoop(args.Length, args, 352, 288, ref fn); + } +#endif + + // Initialize the mesh plugin + public override void Initialise(IMesher meshmerizer, IConfigSource config) + { + mesher = meshmerizer; + m_config = config; + // Defaults + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + avPIDD = 3200.0f; + avPIDP = 1400.0f; + avStandupTensor = 2000000f; + } + else + { + avPIDD = 2200.0f; + avPIDP = 900.0f; + avStandupTensor = 550000f; + } + + int contactsPerCollision = 80; + + if (m_config != null) + { + IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + if (physicsconfig != null) + { + gravityx = physicsconfig.GetFloat("world_gravityx", 0f); + gravityy = physicsconfig.GetFloat("world_gravityy", 0f); + gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f); + + worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4); + worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128); + + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); + smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4); + smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66); + + contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f); + + nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f); + nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f); + nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f); + + mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f); + mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f); + mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f); + + nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f); + nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f); + + mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f); + mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f); + + ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f); + m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); + + avDensity = physicsconfig.GetFloat("av_density", 80f); + avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); + avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f); + avCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false); + + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); + + geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); + geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); + geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); + + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); + bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); + + bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f); + bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f); + + forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); + meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); + meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f); + MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f); + m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f); + avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f); + bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f); + } + else + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); + avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f); + bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f); + } + + physics_logging = physicsconfig.GetBoolean("physics_logging", false); + physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); + physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); + + m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false); + minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); + } + } + + contacts = new d.ContactGeom[contactsPerCollision]; + + staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)]; + + // Centeral contact friction and bounce + // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why + // an avatar falls through in Z but not in X or Y when walking on a prim. + contact.surface.mode |= d.ContactFlags.SoftERP; + contact.surface.mu = nmAvatarObjectContactFriction; + contact.surface.bounce = nmAvatarObjectContactBounce; + contact.surface.soft_cfm = 0.010f; + contact.surface.soft_erp = 0.010f; + + // Terrain contact friction and Bounce + // This is the *non* moving version. Use this when an avatar + // isn't moving to keep it in place better + TerrainContact.surface.mode |= d.ContactFlags.SoftERP; + TerrainContact.surface.mu = nmTerrainContactFriction; + TerrainContact.surface.bounce = nmTerrainContactBounce; + TerrainContact.surface.soft_erp = nmTerrainContactERP; + + WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM); + WaterContact.surface.mu = 0f; // No friction + WaterContact.surface.bounce = 0.0f; // No bounce + WaterContact.surface.soft_cfm = 0.010f; + WaterContact.surface.soft_erp = 0.010f; + + // Prim contact friction and bounce + // THis is the *non* moving version of friction and bounce + // Use this when an avatar comes in contact with a prim + // and is moving + AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction; + AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce; + + // Terrain contact friction bounce and various error correcting calculations + // Use this when an avatar is in contact with the terrain and moving. + AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP; + AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction; + AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce; + AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP; + + + /* + + Stone = 0, + /// + Metal = 1, + /// + Glass = 2, + /// + Wood = 3, + /// + Flesh = 4, + /// + Plastic = 5, + /// + Rubber = 6 + */ + + m_materialContacts = new d.Contact[7,2]; + + m_materialContacts[(int)Material.Stone, 0] = new d.Contact(); + m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Stone, 1] = new d.Contact(); + m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Metal, 0] = new d.Contact(); + m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Metal, 1] = new d.Contact(); + m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Glass, 0] = new d.Contact(); + m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f; + m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f; + m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f; + + /* + private float nmAvatarObjectContactFriction = 250f; + private float nmAvatarObjectContactBounce = 0.1f; + + private float mAvatarObjectContactFriction = 75f; + private float mAvatarObjectContactBounce = 0.1f; + */ + m_materialContacts[(int)Material.Glass, 1] = new d.Contact(); + m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f; + m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f; + m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Wood, 0] = new d.Contact(); + m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Wood, 1] = new d.Contact(); + m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Flesh, 0] = new d.Contact(); + m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Flesh, 1] = new d.Contact(); + m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Plastic, 0] = new d.Contact(); + m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Plastic, 1] = new d.Contact(); + m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Rubber, 0] = new d.Contact(); + m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Rubber, 1] = new d.Contact(); + m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f; + + d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh); + + // Set the gravity,, don't disable things automatically (we set it explicitly on some things) + + d.WorldSetGravity(world, gravityx, gravityy, gravityz); + d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); + + d.WorldSetLinearDamping(world, 256f); + d.WorldSetAngularDamping(world, 256f); + d.WorldSetAngularDampingThreshold(world, 256f); + d.WorldSetLinearDampingThreshold(world, 256f); + d.WorldSetMaxAngularSpeed(world, 256f); + + // Set how many steps we go without running collision testing + // This is in addition to the step size. + // Essentially Steps * m_physicsiterations + d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + //d.WorldSetContactMaxCorrectingVel(world, 1000.0f); + + + + for (int i = 0; i < staticPrimspace.GetLength(0); i++) + { + for (int j = 0; j < staticPrimspace.GetLength(1); j++) + { + staticPrimspace[i, j] = IntPtr.Zero; + } + } + } + + internal void waitForSpaceUnlock(IntPtr space) + { + //if (space != IntPtr.Zero) + //while (d.SpaceLockQuery(space)) { } // Wait and do nothing + } + + /// + /// Debug space message for printing the space that a prim/avatar is in. + /// + /// + /// Returns which split up space the given position is in. + public string whichspaceamIin(Vector3 pos) + { + return calculateSpaceForGeom(pos).ToString(); + } + + #region Collision Detection + + /// + /// This is our near callback. A geometry is near a body + /// + /// The space that contains the geoms. Remember, spaces are also geoms + /// a geometry or space + /// another geometry or space + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + // no lock here! It's invoked from within Simulate(), which is thread-locked + + // Test if we're colliding a geom with a space. + // If so we have to drill down into the space recursively + + if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) + { + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + // Separating static prim geometry spaces. + // We'll be calling near recursivly if one + // of them is a space to find all of the + // contact points in the space + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide test a space"); + return; + } + //Colliding a space or a geom with a space or a geom. so drill down + + //Collide all geoms in each space.. + //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback); + //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback); + return; + } + + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + IntPtr b1 = d.GeomGetBody(g1); + IntPtr b2 = d.GeomGetBody(g2); + + // d.GeomClassID id = d.GeomGetClass(g1); + + String name1 = null; + String name2 = null; + + if (!geom_name_map.TryGetValue(g1, out name1)) + { + name1 = "null"; + } + if (!geom_name_map.TryGetValue(g2, out name2)) + { + name2 = "null"; + } + + //if (id == d.GeomClassId.TriMeshClass) + //{ + // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2); + //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); + //} + + // Figure out how many contact points we have + int count = 0; + try + { + // Colliding Geom To Geom + // This portion of the function 'was' blatantly ripped off from BoxStack.cs + + if (g1 == g2) + return; // Can't collide with yourself + + if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) + return; + + lock (contacts) + { + count = d.Collide(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf); + if (count > contacts.Length) + m_log.Error("[PHYSICS]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length); + } + } + 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; + } + + PhysicsActor p1; + PhysicsActor p2; + + if (!actor_name_map.TryGetValue(g1, out p1)) + { + p1 = PANull; + } + + if (!actor_name_map.TryGetValue(g2, out p2)) + { + p2 = PANull; + } + + ContactPoint maxDepthContact = new ContactPoint(); + if (p1.CollisionScore + count >= float.MaxValue) + p1.CollisionScore = 0; + p1.CollisionScore += count; + + if (p2.CollisionScore + count >= float.MaxValue) + p2.CollisionScore = 0; + p2.CollisionScore += count; + + for (int i = 0; i < count; i++) + { + d.ContactGeom curContact = contacts[i]; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact = new ContactPoint( + new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), + new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), + curContact.depth + ); + } + + //m_log.Warn("[CCOUNT]: " + count); + IntPtr joint; + // If we're colliding with terrain, use 'TerrainContact' instead of contact. + // allows us to have different settings + + // We only need to test p2 for 'jump crouch purposes' + if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim) + { + // Testing if the collision is at the feet of the avatar + + //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); + if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)) + p2.IsColliding = true; + } + else + { + p2.IsColliding = true; + } + + //if ((framecount % m_returncollisions) == 0) + + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Agent: + p2.CollidingObj = true; + break; + case (int)ActorTypes.Prim: + if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + break; + case (int)ActorTypes.Unknown: + p2.CollidingGround = true; + break; + default: + p2.CollidingGround = true; + break; + } + + // we don't want prim or avatar to explode + + #region InterPenetration Handling - Unintended physics explosions +# region disabled code1 + + if (curContact.depth >= 0.08f) + { + //This is disabled at the moment only because it needs more tweaking + //It will eventually be uncommented + /* + if (contact.depth >= 1.00f) + { + //m_log.Debug("[PHYSICS]: " + contact.depth.ToString()); + } + + //If you interpenetrate a prim with an agent + if ((p2.PhysicsActorType == (int) ActorTypes.Agent && + p1.PhysicsActorType == (int) ActorTypes.Prim) || + (p1.PhysicsActorType == (int) ActorTypes.Agent && + p2.PhysicsActorType == (int) ActorTypes.Prim)) + { + + //contact.depth = contact.depth * 4.15f; + /* + if (p2.PhysicsActorType == (int) ActorTypes.Agent) + { + p2.CollidingObj = true; + contact.depth = 0.003f; + p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f); + OdeCharacter character = (OdeCharacter) p2; + character.SetPidStatus(true); + contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2)); + + } + else + { + + //contact.depth = 0.0000000f; + } + if (p1.PhysicsActorType == (int) ActorTypes.Agent) + { + + p1.CollidingObj = true; + contact.depth = 0.003f; + p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f); + contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2)); + OdeCharacter character = (OdeCharacter)p1; + character.SetPidStatus(true); + } + else + { + + //contact.depth = 0.0000000f; + } + + + + } +*/ + // If you interpenetrate a prim with another prim + /* + if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim) + { + #region disabledcode2 + //OdePrim op1 = (OdePrim)p1; + //OdePrim op2 = (OdePrim)p2; + //op1.m_collisionscore++; + //op2.m_collisionscore++; + + //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000) + //{ + //op1.m_taintdisable = true; + //AddPhysicsActorTaint(p1); + //op2.m_taintdisable = true; + //AddPhysicsActorTaint(p2); + //} + + //if (contact.depth >= 0.25f) + //{ + // Don't collide, one or both prim will expld. + + //op1.m_interpenetrationcount++; + //op2.m_interpenetrationcount++; + //interpenetrations_before_disable = 200; + //if (op1.m_interpenetrationcount >= interpenetrations_before_disable) + //{ + //op1.m_taintdisable = true; + //AddPhysicsActorTaint(p1); + //} + //if (op2.m_interpenetrationcount >= interpenetrations_before_disable) + //{ + // op2.m_taintdisable = true; + //AddPhysicsActorTaint(p2); + //} + + //contact.depth = contact.depth / 8f; + //contact.normal = new d.Vector3(0, 0, 1); + //} + //if (op1.m_disabled || op2.m_disabled) + //{ + //Manually disabled objects stay disabled + //contact.depth = 0f; + //} + #endregion + } + */ +#endregion + if (curContact.depth >= 1.00f) + { + //m_log.Info("[P]: " + contact.depth.ToString()); + if ((p2.PhysicsActorType == (int) ActorTypes.Agent && + p1.PhysicsActorType == (int) ActorTypes.Unknown) || + (p1.PhysicsActorType == (int) ActorTypes.Agent && + p2.PhysicsActorType == (int) ActorTypes.Unknown)) + { + if (p2.PhysicsActorType == (int) ActorTypes.Agent) + { + if (p2 is OdeCharacter) + { + OdeCharacter character = (OdeCharacter) p2; + + //p2.CollidingObj = true; + curContact.depth = 0.00000003f; + p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f); + curContact.pos = + new d.Vector3(curContact.pos.X + (p1.Size.X/2), + curContact.pos.Y + (p1.Size.Y/2), + curContact.pos.Z + (p1.Size.Z/2)); + character.SetPidStatus(true); + } + } + + + if (p1.PhysicsActorType == (int) ActorTypes.Agent) + { + if (p1 is OdeCharacter) + { + OdeCharacter character = (OdeCharacter) p1; + + //p2.CollidingObj = true; + curContact.depth = 0.00000003f; + p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f); + curContact.pos = + new d.Vector3(curContact.pos.X + (p1.Size.X/2), + curContact.pos.Y + (p1.Size.Y/2), + curContact.pos.Z + (p1.Size.Z/2)); + character.SetPidStatus(true); + } + } + } + } + } + + #endregion + + // Logic for collision handling + // Note, that if *all* contacts are skipped (VolumeDetect) + // The prim still detects (and forwards) collision events but + // appears to be phantom for the world + Boolean skipThisContact = false; + + if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect)) + skipThisContact = true; // No collision on volume detect prims + + if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) + skipThisContact = true; // No collision on volume detect prims + + if (!skipThisContact && curContact.depth < 0f) + skipThisContact = true; + + if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType)) + skipThisContact = true; + + const int maxContactsbeforedeath = 4000; + joint = IntPtr.Zero; + + if (!skipThisContact) + { + // If we're colliding against terrain + if (name1 == "Terrain" || name2 == "Terrain") + { + // If we're moving + if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && + (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) + { + // Use the movement terrain contact + AvatarMovementTerrainContact.geom = curContact; + _perloopContact.Add(curContact); + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact); + m_global_contactcount++; + } + } + else + { + if (p2.PhysicsActorType == (int)ActorTypes.Agent) + { + // Use the non moving terrain contact + TerrainContact.geom = curContact; + _perloopContact.Add(curContact); + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref TerrainContact); + m_global_contactcount++; + } + } + else + { + if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim) + { + // prim prim contact + // int pj294950 = 0; + int movintYN = 0; + int material = (int) Material.Wood; + // prim terrain contact + if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) + { + movintYN = 1; + } + + if (p2 is OdePrim) + material = ((OdePrim)p2).m_material; + + //m_log.DebugFormat("Material: {0}", material); + m_materialContacts[material, movintYN].geom = curContact; + _perloopContact.Add(curContact); + + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); + m_global_contactcount++; + + } + + } + else + { + + int movintYN = 0; + // prim terrain contact + if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) + { + movintYN = 1; + } + + int material = (int)Material.Wood; + + if (p2 is OdePrim) + material = ((OdePrim)p2).m_material; + //m_log.DebugFormat("Material: {0}", material); + m_materialContacts[material, movintYN].geom = curContact; + _perloopContact.Add(curContact); + + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); + m_global_contactcount++; + + } + } + } + } + //if (p2.PhysicsActorType == (int)ActorTypes.Prim) + //{ + //m_log.Debug("[PHYSICS]: prim contacting with ground"); + //} + } + else if (name1 == "Water" || name2 == "Water") + { + /* + if ((p2.PhysicsActorType == (int) ActorTypes.Prim)) + { + } + else + { + } + */ + //WaterContact.surface.soft_cfm = 0.0000f; + //WaterContact.surface.soft_erp = 0.00000f; + if (curContact.depth > 0.1f) + { + curContact.depth *= 52; + //contact.normal = new d.Vector3(0, 0, 1); + //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f); + } + WaterContact.geom = curContact; + _perloopContact.Add(curContact); + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref WaterContact); + m_global_contactcount++; + } + //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth); + } + else + { + // we're colliding with prim or avatar + // check if we're moving + if ((p2.PhysicsActorType == (int)ActorTypes.Agent)) + { + if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) + { + // Use the Movement prim contact + AvatarMovementprimContact.geom = curContact; + _perloopContact.Add(curContact); + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact); + m_global_contactcount++; + } + } + else + { + // Use the non movement contact + contact.geom = curContact; + _perloopContact.Add(curContact); + + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref contact); + m_global_contactcount++; + } + } + } + else if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + //p1.PhysicsActorType + int material = (int)Material.Wood; + + if (p2 is OdePrim) + material = ((OdePrim)p2).m_material; + + //m_log.DebugFormat("Material: {0}", material); + m_materialContacts[material, 0].geom = curContact; + _perloopContact.Add(curContact); + + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]); + m_global_contactcount++; + + } + } + } + + if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide! + { + d.JointAttach(joint, b1, b2); + m_global_contactcount++; + } + + } + collision_accounting_events(p1, p2, maxDepthContact); + if (count > geomContactPointsStartthrottle) + { + // If there are more then 3 contact points, it's likely + // that we've got a pile of objects, so ... + // We don't want to send out hundreds of terse updates over and over again + // so lets throttle them and send them again after it's somewhat sorted out. + p2.ThrottleUpdates = true; + } + //m_log.Debug(count.ToString()); + //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); + } + } + + private bool checkDupe(d.ContactGeom contactGeom, int atype) + { + bool result = false; + //return result; + if (!m_filterCollisions) + return false; + + ActorTypes at = (ActorTypes)atype; + lock (_perloopContact) + { + foreach (d.ContactGeom contact in _perloopContact) + { + //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2)) + //{ + // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2) + if (at == ActorTypes.Agent) + { + if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom) + { + + if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f) + { + //contactGeom.depth *= .00005f; + //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); + // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); + result = true; + break; + } + else + { + //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); + } + } + else + { + //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); + //int i = 0; + } + } + else if (at == ActorTypes.Prim) + { + //d.AABB aabb1 = new d.AABB(); + //d.AABB aabb2 = new d.AABB(); + + //d.GeomGetAABB(contactGeom.g2, out aabb2); + //d.GeomGetAABB(contactGeom.g1, out aabb1); + //aabb1. + if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom) + { + if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z) + { + if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f) + { + result = true; + break; + } + } + //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); + //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); + } + + } + + //} + + } + } + return result; + } + + private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) + { + // obj1LocalID = 0; + //returncollisions = false; + obj2LocalID = 0; + //ctype = 0; + //cStartStop = 0; + if (!p2.SubscribedEvents() && !p1.SubscribedEvents()) + return; + + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + cc2 = (OdeCharacter)p2; + + // obj1LocalID = cc2.m_localID; + switch ((ActorTypes)p1.PhysicsActorType) + { + case ActorTypes.Agent: + cc1 = (OdeCharacter)p1; + obj2LocalID = cc1.m_localID; + cc1.AddCollisionEvent(cc2.m_localID, contact); + //ctype = (int)CollisionCategories.Character; + + //if (cc1.CollidingObj) + //cStartStop = (int)StatusIndicators.Generic; + //else + //cStartStop = (int)StatusIndicators.Start; + + //returncollisions = true; + break; + case ActorTypes.Prim: + if (p1 is OdePrim) + { + cp1 = (OdePrim) p1; + obj2LocalID = cp1.m_localID; + cp1.AddCollisionEvent(cc2.m_localID, contact); + } + //ctype = (int)CollisionCategories.Geom; + + //if (cp1.CollidingObj) + //cStartStop = (int)StatusIndicators.Generic; + //else + //cStartStop = (int)StatusIndicators.Start; + + //returncollisions = true; + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + obj2LocalID = 0; + //ctype = (int)CollisionCategories.Land; + //returncollisions = true; + break; + } + + cc2.AddCollisionEvent(obj2LocalID, contact); + break; + case ActorTypes.Prim: + + if (p2 is OdePrim) + { + cp2 = (OdePrim) p2; + + // obj1LocalID = cp2.m_localID; + switch ((ActorTypes) p1.PhysicsActorType) + { + case ActorTypes.Agent: + if (p1 is OdeCharacter) + { + cc1 = (OdeCharacter) p1; + obj2LocalID = cc1.m_localID; + cc1.AddCollisionEvent(cp2.m_localID, contact); + //ctype = (int)CollisionCategories.Character; + + //if (cc1.CollidingObj) + //cStartStop = (int)StatusIndicators.Generic; + //else + //cStartStop = (int)StatusIndicators.Start; + //returncollisions = true; + } + break; + case ActorTypes.Prim: + + if (p1 is OdePrim) + { + cp1 = (OdePrim) p1; + obj2LocalID = cp1.m_localID; + cp1.AddCollisionEvent(cp2.m_localID, contact); + //ctype = (int)CollisionCategories.Geom; + + //if (cp1.CollidingObj) + //cStartStop = (int)StatusIndicators.Generic; + //else + //cStartStop = (int)StatusIndicators.Start; + + //returncollisions = true; + } + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + obj2LocalID = 0; + //ctype = (int)CollisionCategories.Land; + + //returncollisions = true; + break; + } + + cp2.AddCollisionEvent(obj2LocalID, contact); + } + break; + } + //if (returncollisions) + //{ + + //lock (m_storedCollisions) + //{ + //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString(); + //if (m_storedCollisions.ContainsKey(cDictKey)) + //{ + //sCollisionData objd = m_storedCollisions[cDictKey]; + //objd.NumberOfCollisions += 1; + //objd.lastframe = framecount; + //m_storedCollisions[cDictKey] = objd; + //} + //else + //{ + //sCollisionData objd = new sCollisionData(); + //objd.ColliderLocalId = obj1LocalID; + //objd.CollidedWithLocalId = obj2LocalID; + //objd.CollisionType = ctype; + //objd.NumberOfCollisions = 1; + //objd.lastframe = framecount; + //objd.StatusIndicator = cStartStop; + //m_storedCollisions.Add(cDictKey, objd); + //} + //} + // } + } + + public int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount) + { + /* String name1 = null; + String name2 = null; + + if (!geom_name_map.TryGetValue(trimesh, out name1)) + { + name1 = "null"; + } + if (!geom_name_map.TryGetValue(refObject, out name2)) + { + name2 = "null"; + } + + m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2); + */ + return 1; + } + + public int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex) + { + String name1 = null; + String name2 = null; + + if (!geom_name_map.TryGetValue(trimesh, out name1)) + { + name1 = "null"; + } + + if (!geom_name_map.TryGetValue(refObject, out name2)) + { + name2 = "null"; + } + + // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex); + + d.Vector3 v0 = new d.Vector3(); + d.Vector3 v1 = new d.Vector3(); + d.Vector3 v2 = new d.Vector3(); + + d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2); + // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z); + + return 1; + } + + /// + /// This is our collision testing routine in ODE + /// + /// + private void collision_optimized(float timeStep) + { + _perloopContact.Clear(); + + lock (_characters) + { + foreach (OdeCharacter chr in _characters) + { + // Reset the collision values to false + // since we don't know if we're colliding yet + + // For some reason this can happen. Don't ask... + // + if (chr == null) + continue; + + if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) + continue; + + chr.IsColliding = false; + chr.CollidingGround = false; + chr.CollidingObj = false; + + // test the avatar's geometry for collision with the space + // This will return near and the space that they are the closest to + // And we'll run this again against the avatar and the space segment + // This will return with a bunch of possible objects in the space segment + // and we'll run it again on all of them. + try + { + d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to space collide"); + } + //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y); + //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10) + //{ + //chr.Position.Z = terrainheight + 10.0f; + //forcedZ = true; + //} + } + } + + lock (_activeprims) + { + List removeprims = null; + foreach (OdePrim chr in _activeprims) + { + if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled)) + { + try + { + lock (chr) + { + if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false) + { + d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback); + } + else + { + if (removeprims == null) + { + removeprims = new List(); + } + removeprims.Add(chr); + m_log.Debug("[PHYSICS]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!"); + } + } + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to space collide"); + } + } + } + if (removeprims != null) + { + foreach (OdePrim chr in removeprims) + { + _activeprims.Remove(chr); + } + } + } + + _perloopContact.Clear(); + } + + #endregion + + public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) + { + m_worldOffset = offset; + WorldExtents = new Vector2(extents.X, extents.Y); + m_parentScene = pScene; + + } + + // Recovered for use by fly height. Kitto Flora + public float GetTerrainHeightAtXY(float x, float y) + { + + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + + IntPtr heightFieldGeom = IntPtr.Zero; + + if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom)) + { + if (heightFieldGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + { + + int index; + + + if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y || + (int)x < 0.001f || (int)y < 0.001f) + return 0; + + x = x - offsetX; + y = y - offsetY; + + index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y); + + if (index < TerrainHeightFieldHeights[heightFieldGeom].Length) + { + //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]); + return (float)TerrainHeightFieldHeights[heightFieldGeom][index]; + } + + else + return 0f; + } + else + { + return 0f; + } + + } + else + { + return 0f; + } + + } + else + { + return 0f; + } + + + } +// End recovered. Kitto Flora + + public void addCollisionEventReporting(PhysicsActor obj) + { + lock (_collisionEventPrim) + { + if (!_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Add(obj); + } + } + + public void remCollisionEventReporting(PhysicsActor obj) + { + lock (_collisionEventPrim) + { + if (!_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Remove(obj); + } + } + + #region Add/Remove Entities + + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) + { + Vector3 pos; + pos.X = position.X; + pos.Y = position.Y; + pos.Z = position.Z; + OdeCharacter newAv = new OdeCharacter(avName, this, pos, ode, size, avPIDD, avPIDP, avCapRadius, avStandupTensor, avDensity, avHeightFudgeFactor, avMovementDivisorWalk, avMovementDivisorRun); + newAv.Flying = isFlying; + newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; + + return newAv; + } + + public void AddCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (!_characters.Contains(chr)) + { + _characters.Add(chr); + if (chr.bad) + m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); + } + } + } + + public void RemoveCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (_characters.Contains(chr)) + { + _characters.Remove(chr); + } + } + } + public void BadCharacter(OdeCharacter chr) + { + lock (_badCharacter) + { + if (!_badCharacter.Contains(chr)) + _badCharacter.Add(chr); + } + } + + public override void RemoveAvatar(PhysicsActor actor) + { + //m_log.Debug("[PHYSICS]:ODELOCK"); + ((OdeCharacter) actor).Destroy(); + + } + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + IMesh mesh, PrimitiveBaseShape pbs, bool isphysical) + { + + Vector3 pos = position; + Vector3 siz = size; + Quaternion rot = rotation; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode); + + lock (_prims) + _prims.Add(newPrim); + } + + return newPrim; + } + + public void addActivePrim(OdePrim activatePrim) + { + // adds active prim.. (ones that should be iterated over in collisions_optimized + lock (_activeprims) + { + if (!_activeprims.Contains(activatePrim)) + _activeprims.Add(activatePrim); + //else + // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); + } + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation) //To be removed + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) + { + PhysicsActor result; + IMesh mesh = null; + + if (needsMeshing(pbs)) + mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical); + + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical); + + return result; + } + + public override float TimeDilation + { + get { return m_timeDilation; } + } + + public override bool SupportsNINJAJoints + { + get { return m_NINJA_physics_joints_enabled; } + } + + // internal utility function: must be called within a lock (OdeLock) + private void InternalAddActiveJoint(PhysicsJoint joint) + { + activeJoints.Add(joint); + SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint); + } + + // internal utility function: must be called within a lock (OdeLock) + private void InternalAddPendingJoint(OdePhysicsJoint joint) + { + pendingJoints.Add(joint); + SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint); + } + + // internal utility function: must be called within a lock (OdeLock) + private void InternalRemovePendingJoint(PhysicsJoint joint) + { + pendingJoints.Remove(joint); + SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene); + } + + // internal utility function: must be called within a lock (OdeLock) + private void InternalRemoveActiveJoint(PhysicsJoint joint) + { + activeJoints.Remove(joint); + SOPName_to_activeJoint.Remove(joint.ObjectNameInScene); + } + + public override void DumpJointInfo() + { + string hdr = "[NINJA] JOINTINFO: "; + foreach (PhysicsJoint j in pendingJoints) + { + m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); + } + m_log.Debug(hdr + pendingJoints.Count + " total pending joints"); + foreach (string jointName in SOPName_to_pendingJoint.Keys) + { + m_log.Debug(hdr + " pending joints dict contains Name: " + jointName); + } + m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries"); + foreach (PhysicsJoint j in activeJoints) + { + m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); + } + m_log.Debug(hdr + activeJoints.Count + " total active joints"); + foreach (string jointName in SOPName_to_activeJoint.Keys) + { + m_log.Debug(hdr + " active joints dict contains Name: " + jointName); + } + m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries"); + + m_log.Debug(hdr + " Per-body joint connectivity information follows."); + m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints."); + foreach (string actorName in joints_connecting_actor.Keys) + { + m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it"); + foreach (PhysicsJoint j in joints_connecting_actor[actorName]) + { + m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); + } + m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor"); + } + } + + public override void RequestJointDeletion(string ObjectNameInScene) + { + lock (externalJointRequestsLock) + { + if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously + { + requestedJointsToBeDeleted.Add(ObjectNameInScene); + } + } + } + + private void DeleteRequestedJoints() + { + List myRequestedJointsToBeDeleted; + lock (externalJointRequestsLock) + { + // make a local copy of the shared list for processing (threading issues) + myRequestedJointsToBeDeleted = new List(requestedJointsToBeDeleted); + } + + foreach (string jointName in myRequestedJointsToBeDeleted) + { + lock (OdeLock) + { + //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName); + if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName)) + { + OdePhysicsJoint joint = null; + if (SOPName_to_activeJoint.ContainsKey(jointName)) + { + joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint; + InternalRemoveActiveJoint(joint); + } + else if (SOPName_to_pendingJoint.ContainsKey(jointName)) + { + joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint; + InternalRemovePendingJoint(joint); + } + + if (joint != null) + { + //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames); + for (int iBodyName = 0; iBodyName < 2; iBodyName++) + { + string bodyName = joint.BodyNames[iBodyName]; + if (bodyName != "NULL") + { + joints_connecting_actor[bodyName].Remove(joint); + if (joints_connecting_actor[bodyName].Count == 0) + { + joints_connecting_actor.Remove(bodyName); + } + } + } + + DoJointDeactivated(joint); + if (joint.jointID != IntPtr.Zero) + { + d.JointDestroy(joint.jointID); + joint.jointID = IntPtr.Zero; + //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName); + } + else + { + //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName); + } + } + else + { + // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName); + } + } + else + { + // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName); + } + } + } + + // remove processed joints from the shared list + lock (externalJointRequestsLock) + { + foreach (string jointName in myRequestedJointsToBeDeleted) + { + requestedJointsToBeDeleted.Remove(jointName); + } + } + } + + // for pending joints we don't know if their associated bodies exist yet or not. + // the joint is actually created during processing of the taints + private void CreateRequestedJoints() + { + List myRequestedJointsToBeCreated; + lock (externalJointRequestsLock) + { + // make a local copy of the shared list for processing (threading issues) + myRequestedJointsToBeCreated = new List(requestedJointsToBeCreated); + } + + foreach (PhysicsJoint joint in myRequestedJointsToBeCreated) + { + lock (OdeLock) + { + if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null) + { + DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation); + continue; + } + if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null) + { + DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation); + continue; + } + + InternalAddPendingJoint(joint as OdePhysicsJoint); + + if (joint.BodyNames.Count >= 2) + { + for (int iBodyName = 0; iBodyName < 2; iBodyName++) + { + string bodyName = joint.BodyNames[iBodyName]; + if (bodyName != "NULL") + { + if (!joints_connecting_actor.ContainsKey(bodyName)) + { + joints_connecting_actor.Add(bodyName, new List()); + } + joints_connecting_actor[bodyName].Add(joint); + } + } + } + } + } + + // remove processed joints from shared list + lock (externalJointRequestsLock) + { + foreach (PhysicsJoint joint in myRequestedJointsToBeCreated) + { + requestedJointsToBeCreated.Remove(joint); + } + } + + } + + // public function to add an request for joint creation + // this joint will just be added to a waiting list that is NOT processed during the main + // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests. + + public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, Vector3 position, + Quaternion rotation, string parms, List bodyNames, string trackedBodyName, Quaternion localRotation) + + { + + OdePhysicsJoint joint = new OdePhysicsJoint(); + joint.ObjectNameInScene = objectNameInScene; + joint.Type = jointType; + joint.Position = position; + joint.Rotation = rotation; + joint.RawParams = parms; + joint.BodyNames = new List(bodyNames); + joint.TrackedBodyName = trackedBodyName; + joint.LocalRotation = localRotation; + joint.jointID = IntPtr.Zero; + joint.ErrorMessageCount = 0; + + lock (externalJointRequestsLock) + { + if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice + { + requestedJointsToBeCreated.Add(joint); + } + } + return joint; + } + + private void RemoveAllJointsConnectedToActor(PhysicsActor actor) + { + //m_log.Debug("RemoveAllJointsConnectedToActor: start"); + if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null) + { + + List jointsToRemove = new List(); + //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism) + foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName]) + { + jointsToRemove.Add(j); + } + foreach (PhysicsJoint j in jointsToRemove) + { + //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene); + RequestJointDeletion(j.ObjectNameInScene); + //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene); + j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing) + } + } + } + + public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor) + { + //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start"); + lock (OdeLock) + { + //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock"); + RemoveAllJointsConnectedToActor(actor); + } + } + + // normally called from within OnJointMoved, which is called from within a lock (OdeLock) + public override Vector3 GetJointAnchor(PhysicsJoint joint) + { + Debug.Assert(joint.IsInPhysicsEngine); + d.Vector3 pos = new d.Vector3(); + + if (!(joint is OdePhysicsJoint)) + { + DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); + } + else + { + OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint; + switch (odeJoint.Type) + { + case PhysicsJointType.Ball: + d.JointGetBallAnchor(odeJoint.jointID, out pos); + break; + case PhysicsJointType.Hinge: + d.JointGetHingeAnchor(odeJoint.jointID, out pos); + break; + } + } + return new Vector3(pos.X, pos.Y, pos.Z); + } + + // normally called from within OnJointMoved, which is called from within a lock (OdeLock) + // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function + // appears to be unreliable. Fortunately we can compute the joint axis ourselves by + // keeping track of the joint's original orientation relative to one of the involved bodies. + public override Vector3 GetJointAxis(PhysicsJoint joint) + { + Debug.Assert(joint.IsInPhysicsEngine); + d.Vector3 axis = new d.Vector3(); + + if (!(joint is OdePhysicsJoint)) + { + DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); + } + else + { + OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint; + switch (odeJoint.Type) + { + case PhysicsJointType.Ball: + DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene); + break; + case PhysicsJointType.Hinge: + d.JointGetHingeAxis(odeJoint.jointID, out axis); + break; + } + } + return new Vector3(axis.X, axis.Y, axis.Z); + } + + + public void remActivePrim(OdePrim deactivatePrim) + { + lock (_activeprims) + { + _activeprims.Remove(deactivatePrim); + } + } + + public override void RemovePrim(PhysicsActor prim) + { + if (prim is OdePrim) + { + lock (OdeLock) + { + OdePrim p = (OdePrim) prim; + + p.setPrimForRemoval(); + AddPhysicsActorTaint(prim); + //RemovePrimThreadLocked(p); + } + } + } + + /// + /// This is called from within simulate but outside the locked portion + /// We need to do our own locking here + /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. + /// + /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory + /// that the space was using. + /// + /// + public void RemovePrimThreadLocked(OdePrim prim) + { +//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); + lock (prim) + { + remCollisionEventReporting(prim); + lock (ode) + { + if (prim.prim_geom != IntPtr.Zero) + { + prim.ResetTaints(); + + if (prim.IsPhysical) + { + prim.disableBody(); + if (prim.childPrim) + { + prim.childPrim = false; + prim.Body = IntPtr.Zero; + prim.m_disabled = true; + prim.IsPhysical = false; + } + + + } + // we don't want to remove the main space + + // If the geometry is in the targetspace, remove it from the target space + //m_log.Warn(prim.m_targetSpace); + + //if (prim.m_targetSpace != IntPtr.Zero) + //{ + //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom)) + //{ + + //if (d.GeomIsSpace(prim.m_targetSpace)) + //{ + //waitForSpaceUnlock(prim.m_targetSpace); + //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom); + prim.m_targetSpace = IntPtr.Zero; + //} + //else + //{ + // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + + //((OdePrim)prim).m_targetSpace.ToString()); + //} + + //} + //} + //m_log.Warn(prim.prim_geom); + try + { + if (prim.prim_geom != IntPtr.Zero) + { + d.GeomDestroy(prim.prim_geom); + prim.prim_geom = IntPtr.Zero; + } + else + { + m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene"); + } + } + catch (AccessViolationException) + { + m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed."); + } + lock (_prims) + _prims.Remove(prim); + + //If there are no more geometries in the sub-space, we don't need it in the main space anymore + //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0) + //{ + //if (prim.m_targetSpace != null) + //{ + //if (d.GeomIsSpace(prim.m_targetSpace)) + //{ + //waitForSpaceUnlock(prim.m_targetSpace); + //d.SpaceRemove(space, prim.m_targetSpace); + // free up memory used by the space. + //d.SpaceDestroy(prim.m_targetSpace); + //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position); + //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]); + //} + //else + //{ + //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + + //((OdePrim) prim).m_targetSpace.ToString()); + //} + //} + //} + + if (SupportsNINJAJoints) + { + RemoveAllJointsConnectedToActorThreadLocked(prim); + } + } + } + } + } + + #endregion + + #region Space Separation Calculation + + /// + /// Takes a space pointer and zeros out the array we're using to hold the spaces + /// + /// + public void resetSpaceArrayItemToZero(IntPtr pSpace) + { + for (int x = 0; x < staticPrimspace.GetLength(0); x++) + { + for (int y = 0; y < staticPrimspace.GetLength(1); y++) + { + if (staticPrimspace[x, y] == pSpace) + staticPrimspace[x, y] = IntPtr.Zero; + } + } + } + + public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY) + { + staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero; + } + + /// + /// Called when a static prim moves. Allocates a space for the prim based on its position + /// + /// the pointer to the geom that moved + /// the position that the geom moved to + /// a pointer to the space it was in before it was moved. + /// a pointer to the new space it's in + public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace) + { + // Called from setting the Position and Size of an ODEPrim so + // it's already in locked space. + + // we don't want to remove the main space + // we don't need to test physical here because this function should + // never be called if the prim is physical(active) + + // All physical prim end up in the root space + //Thread.Sleep(20); + if (currentspace != space) + { + //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString()); + //if (currentspace == IntPtr.Zero) + //{ + //int adfadf = 0; + //} + if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + else + { + m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace + + " Geom:" + geom); + } + } + else + { + IntPtr sGeomIsIn = d.GeomGetSpace(geom); + if (sGeomIsIn != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(sGeomIsIn); + d.SpaceRemove(sGeomIsIn, geom); + } + else + { + m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + + sGeomIsIn + " Geom:" + geom); + } + } + } + + //If there are no more geometries in the sub-space, we don't need it in the main space anymore + if (d.SpaceGetNumGeoms(currentspace) == 0) + { + if (currentspace != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + waitForSpaceUnlock(space); + d.SpaceRemove(space, currentspace); + // free up memory used by the space. + + //d.SpaceDestroy(currentspace); + resetSpaceArrayItemToZero(currentspace); + } + else + { + m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + + currentspace + " Geom:" + geom); + } + } + } + } + else + { + // this is a physical object that got disabled. ;.; + if (currentspace != IntPtr.Zero && geom != IntPtr.Zero) + { + if (d.SpaceQuery(currentspace, geom)) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + else + { + m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + + currentspace + " Geom:" + geom); + } + } + else + { + IntPtr sGeomIsIn = d.GeomGetSpace(geom); + if (sGeomIsIn != IntPtr.Zero) + { + if (d.GeomIsSpace(sGeomIsIn)) + { + waitForSpaceUnlock(sGeomIsIn); + d.SpaceRemove(sGeomIsIn, geom); + } + else + { + m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + + sGeomIsIn + " Geom:" + geom); + } + } + } + } + } + + // The routines in the Position and Size sections do the 'inserting' into the space, + // so all we have to do is make sure that the space that we're putting the prim into + // is in the 'main' space. + int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos); + IntPtr newspace = calculateSpaceForGeom(pos); + + if (newspace == IntPtr.Zero) + { + newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); + d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh); + } + + return newspace; + } + + /// + /// Creates a new space at X Y + /// + /// + /// + /// A pointer to the created space + public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY) + { + // creating a new space for prim and inserting it into main space. + staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero); + d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space); + waitForSpaceUnlock(space); + d.SpaceSetSublevel(space, 1); + d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]); + return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]; + } + + /// + /// Calculates the space the prim should be in by its position + /// + /// + /// a pointer to the space. This could be a new space or reused space. + public IntPtr calculateSpaceForGeom(Vector3 pos) + { + int[] xyspace = calculateSpaceArrayItemFromPos(pos); + //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString()); + return staticPrimspace[xyspace[0], xyspace[1]]; + } + + /// + /// Holds the space allocation logic + /// + /// + /// an array item based on the position + public int[] calculateSpaceArrayItemFromPos(Vector3 pos) + { + int[] returnint = new int[2]; + + returnint[0] = (int) (pos.X/metersInSpace); + + if (returnint[0] > ((int) (259f/metersInSpace))) + returnint[0] = ((int) (259f/metersInSpace)); + if (returnint[0] < 0) + returnint[0] = 0; + + returnint[1] = (int) (pos.Y/metersInSpace); + if (returnint[1] > ((int) (259f/metersInSpace))) + returnint[1] = ((int) (259f/metersInSpace)); + if (returnint[1] < 0) + returnint[1] = 0; + + return returnint; + } + + #endregion + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// + public bool needsMeshing(PrimitiveBaseShape pbs) + { + // most of this is redundant now as the mesher will return null if it cant mesh a prim + // but we still need to check for sculptie meshing being enabled so this is the most + // convenient place to do it for now... + + // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f) + // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString()); + int iPropertiesNotSupportedDefault = 0; + + if (pbs.SculptEntry && !meshSculptedPrim) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } + + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim + if (!forceSimplePrimMeshing) + { + if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) + || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 + && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) + { + + if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 + && pbs.ProfileHollow == 0 + && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 + && pbs.PathBegin == 0 && pbs.PathEnd == 0 + && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 + && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 + && pbs.PathShearX == 0 && pbs.PathShearY == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } + } + } + + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + + + if (iPropertiesNotSupportedDefault == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } +#if SPAM + m_log.Debug("Mesh"); +#endif + return true; + } + + /// + /// Called after our prim properties are set Scale, position etc. + /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex + /// This assures us that we have no race conditions + /// + /// + public override void AddPhysicsActorTaint(PhysicsActor prim) + { + + if (prim is OdePrim) + { + OdePrim taintedprim = ((OdePrim) prim); + lock (_taintedPrimLock) + { + if (!(_taintedPrimH.Contains(taintedprim))) + { +//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName); + _taintedPrimH.Add(taintedprim); // HashSet for searching + _taintedPrimL.Add(taintedprim); // List for ordered readout + } + } + return; + } + else if (prim is OdeCharacter) + { + OdeCharacter taintedchar = ((OdeCharacter)prim); + lock (_taintedActors) + { + if (!(_taintedActors.Contains(taintedchar))) + { + _taintedActors.Add(taintedchar); + if (taintedchar.bad) + m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid); + } + } + } + } + + /// + /// This is our main simulate loop + /// It's thread locked by a Mutex in the scene. + /// It holds Collisions, it instructs ODE to step through the physical reactions + /// It moves the objects around in memory + /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) + /// + /// + /// + public override float Simulate(float timeStep) + { + if (framecount >= int.MaxValue) + framecount = 0; + + //if (m_worldOffset != Vector3.Zero) + // return 0; + + framecount++; + + float fps = 0; + //m_log.Info(timeStep.ToString()); + step_time += timeStep; + + // If We're loaded down by something else, + // or debugging with the Visual Studio project on pause + // skip a few frames to catch up gracefully. + // without shooting the physicsactors all over the place + + if (step_time >= m_SkipFramesAtms) + { + // Instead of trying to catch up, it'll do 5 physics frames only + step_time = ODE_STEPSIZE; + m_physicsiterations = 5; + } + else + { + m_physicsiterations = 10; + } + + if (SupportsNINJAJoints) + { + DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks + CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks + } + + lock (OdeLock) + { + // Process 10 frames if the sim is running normal.. + // process 5 frames if the sim is running slow + //try + //{ + //d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + //} + //catch (StackOverflowException) + //{ + // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); + // ode.drelease(world); + //base.TriggerPhysicsBasedRestart(); + //} + + int i = 0; + + // Figure out the Frames Per Second we're going at. + //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size + + fps = (step_time / ODE_STEPSIZE) * 1000; + // HACK: Using a time dilation of 1.0 to debug rubberbanding issues + //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f); + + step_time = 0.09375f; + + while (step_time > 0.0f) + { + //lock (ode) + //{ + //if (!ode.lockquery()) + //{ + // ode.dlock(world); + try + { + // Insert, remove Characters + bool processedtaints = false; + + lock (_taintedActors) + { + if (_taintedActors.Count > 0) + { + foreach (OdeCharacter character in _taintedActors) + { + + character.ProcessTaints(timeStep); + + processedtaints = true; + //character.m_collisionscore = 0; + } + + if (processedtaints) + _taintedActors.Clear(); + } + } + + // Modify other objects in the scene. + processedtaints = false; + + lock (_taintedPrimLock) + { + foreach (OdePrim prim in _taintedPrimL) + { + if (prim.m_taintremove) + { + //Console.WriteLine("Simulate calls RemovePrimThreadLocked"); + RemovePrimThreadLocked(prim); + } + else + { + //Console.WriteLine("Simulate calls ProcessTaints"); + prim.ProcessTaints(timeStep); + } + processedtaints = true; + prim.m_collisionscore = 0; + + // This loop can block up the Heartbeat for a very long time on large regions. + // We need to let the Watchdog know that the Heartbeat is not dead + // NOTE: This is currently commented out, but if things like OAR loading are + // timing the heartbeat out we will need to uncomment it + //Watchdog.UpdateThread(); + } + + if (SupportsNINJAJoints) + { + // Create pending joints, if possible + + // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating + // a joint requires specifying the body id of both involved bodies + if (pendingJoints.Count > 0) + { + List successfullyProcessedPendingJoints = new List(); + //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints"); + foreach (PhysicsJoint joint in pendingJoints) + { + //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams); + string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries); + List jointBodies = new List(); + bool allJointBodiesAreReady = true; + foreach (string jointParam in jointParams) + { + if (jointParam == "NULL") + { + //DoJointErrorMessage(joint, "attaching NULL joint to world"); + jointBodies.Add(IntPtr.Zero); + } + else + { + //DoJointErrorMessage(joint, "looking for prim name: " + jointParam); + bool foundPrim = false; + lock (_prims) + { + foreach (OdePrim prim in _prims) // FIXME: inefficient + { + if (prim.SOPName == jointParam) + { + //DoJointErrorMessage(joint, "found for prim name: " + jointParam); + if (prim.IsPhysical && prim.Body != IntPtr.Zero) + { + jointBodies.Add(prim.Body); + foundPrim = true; + break; + } + else + { + DoJointErrorMessage(joint, "prim name " + jointParam + + " exists but is not (yet) physical; deferring joint creation. " + + "IsPhysical property is " + prim.IsPhysical + + " and body is " + prim.Body); + foundPrim = false; + break; + } + } + } + } + if (foundPrim) + { + // all is fine + } + else + { + allJointBodiesAreReady = false; + break; + } + } + } + if (allJointBodiesAreReady) + { + //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams); + if (jointBodies[0] == jointBodies[1]) + { + DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams); + } + else + { + switch (joint.Type) + { + case PhysicsJointType.Ball: + { + IntPtr odeJoint; + //DoJointErrorMessage(joint, "ODE creating ball joint "); + odeJoint = d.JointCreateBall(world, IntPtr.Zero); + //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]); + d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]); + //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position); + d.JointSetBallAnchor(odeJoint, + joint.Position.X, + joint.Position.Y, + joint.Position.Z); + //DoJointErrorMessage(joint, "ODE joint setting OK"); + //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: "); + //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment")); + //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: "); + //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment")); + + if (joint is OdePhysicsJoint) + { + ((OdePhysicsJoint)joint).jointID = odeJoint; + } + else + { + DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!"); + } + } + break; + case PhysicsJointType.Hinge: + { + IntPtr odeJoint; + //DoJointErrorMessage(joint, "ODE creating hinge joint "); + odeJoint = d.JointCreateHinge(world, IntPtr.Zero); + //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]); + d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]); + //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position); + d.JointSetHingeAnchor(odeJoint, + joint.Position.X, + joint.Position.Y, + joint.Position.Z); + // We use the orientation of the x-axis of the joint's coordinate frame + // as the axis for the hinge. + + // Therefore, we must get the joint's coordinate frame based on the + // joint.Rotation field, which originates from the orientation of the + // joint's proxy object in the scene. + + // The joint's coordinate frame is defined as the transformation matrix + // that converts a vector from joint-local coordinates into world coordinates. + // World coordinates are defined as the XYZ coordinate system of the sim, + // as shown in the top status-bar of the viewer. + + // Once we have the joint's coordinate frame, we extract its X axis (AtAxis) + // and use that as the hinge axis. + + //joint.Rotation.Normalize(); + Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation); + + // Now extract the X axis of the joint's coordinate frame. + + // Do not try to use proxyFrame.AtAxis or you will become mired in the + // tar pit of transposed, inverted, and generally messed-up orientations. + // (In other words, Matrix4.AtAxis() is borked.) + // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness + + // Instead, compute the X axis of the coordinate frame by transforming + // the (1,0,0) vector. At least that works. + + //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame); + Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame); + //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis); + //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis); + d.JointSetHingeAxis(odeJoint, + jointAxis.X, + jointAxis.Y, + jointAxis.Z); + //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f); + if (joint is OdePhysicsJoint) + { + ((OdePhysicsJoint)joint).jointID = odeJoint; + } + else + { + DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!"); + } + } + break; + } + successfullyProcessedPendingJoints.Add(joint); + } + } + else + { + DoJointErrorMessage(joint, "joint could not yet be created; still pending"); + } + } + foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints) + { + //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams); + //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending"); + InternalRemovePendingJoint(successfullyProcessedJoint); + //DoJointErrorMessage(successfullyProcessedJoint, "adding to active"); + InternalAddActiveJoint(successfullyProcessedJoint); + //DoJointErrorMessage(successfullyProcessedJoint, "done"); + } + } + } + + if (processedtaints) +//Console.WriteLine("Simulate calls Clear of _taintedPrim list"); + _taintedPrimH.Clear(); + _taintedPrimL.Clear(); + } + + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(timeStep, defects); + } + if (0 != defects.Count) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + } + } + + // Move other active objects + lock (_activeprims) + { + foreach (OdePrim prim in _activeprims) + { + prim.m_collisionscore = 0; + prim.Move(timeStep); + } + } + + //if ((framecount % m_randomizeWater) == 0) + // randomizeWater(waterlevel); + + //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests(); + m_rayCastManager.ProcessQueuedRequests(); + + collision_optimized(timeStep); + + lock (_collisionEventPrim) + { + foreach (PhysicsActor obj in _collisionEventPrim) + { + if (obj == null) + continue; + + switch ((ActorTypes)obj.PhysicsActorType) + { + case ActorTypes.Agent: + OdeCharacter cobj = (OdeCharacter)obj; + cobj.AddCollisionFrameTime(100); + cobj.SendCollisions(); + break; + case ActorTypes.Prim: + OdePrim pobj = (OdePrim)obj; + pobj.SendCollisions(); + break; + } + } + } + + //if (m_global_contactcount > 5) + //{ + // m_log.DebugFormat("[PHYSICS]: Contacts:{0}", m_global_contactcount); + //} + + m_global_contactcount = 0; + + d.WorldQuickStep(world, ODE_STEPSIZE); + d.JointGroupEmpty(contactgroup); + //ode.dunlock(world); + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); + ode.dunlock(world); + } + + step_time -= ODE_STEPSIZE; + i++; + //} + //else + //{ + //fps = 0; + //} + //} + } + + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); + actor.UpdatePositionAndVelocity(); + } + } + } + + lock (_badCharacter) + { + if (_badCharacter.Count > 0) + { + foreach (OdeCharacter chr in _badCharacter) + { + RemoveCharacter(chr); + } + _badCharacter.Clear(); + } + } + + lock (_activeprims) + { + //if (timeStep < 0.2f) + { + foreach (OdePrim actor in _activeprims) + { + if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag)) + { + actor.UpdatePositionAndVelocity(); + + if (SupportsNINJAJoints) + { + // If an actor moved, move its joint proxy objects as well. + // There seems to be an event PhysicsActor.OnPositionUpdate that could be used + // for this purpose but it is never called! So we just do the joint + // movement code here. + + if (actor.SOPName != null && + joints_connecting_actor.ContainsKey(actor.SOPName) && + joints_connecting_actor[actor.SOPName] != null && + joints_connecting_actor[actor.SOPName].Count > 0) + { + foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName]) + { + if (affectedJoint.IsInPhysicsEngine) + { + DoJointMoved(affectedJoint); + } + else + { + DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams); + } + } + } + } + } + } + } + } + + //DumpJointInfo(); + + // Finished with all sim stepping. If requested, dump world state to file for debugging. + // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? + // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? + if (physics_logging && (physics_logging_interval>0) && (framecount % physics_logging_interval == 0)) + { + string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename + string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file + + if (physics_logging_append_existing_logfile) + { + string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------"; + TextWriter fwriter = File.AppendText(fname); + fwriter.WriteLine(header); + fwriter.Close(); + } + d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); + } + } + + return fps; + } + + public override void GetResults() + { + } + + public override bool IsThreaded + { + // for now we won't be multithreaded + get { return (false); } + } + + #region ODE Specific Terrain Fixes + public float[] ResizeTerrain512NearestNeighbour(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < WorldExtents.Y; y++) + { + for (int x = 0; x < WorldExtents.X; x++) + { + resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x]; + } + } + + // Resize using Nearest Neighbour + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512, 512]; + for (int y = 0; y < WorldExtents.Y; y++) + { + for (int x = 0; x < WorldExtents.X; x++) + { + resultarr2[y * 2, x * 2] = resultarr[y, x]; + + if (y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x]; + } + if (x < WorldExtents.X) + { + resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x]; + } + if (x < WorldExtents.X && y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x]; + } + } + } + + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) + { + if (resultarr2[y, x] <= 0) + returnarr[i] = 0.0000001f; + else + returnarr[i] = resultarr2[y, x]; + + i++; + } + } + + return returnarr; + } + + public float[] ResizeTerrain512Interpolation(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[512,512]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < 256; y++) + { + for (int x = 0; x < 256; x++) + { + resultarr[y, x] = heightMap[y * 256 + x]; + } + } + + // Resize using interpolation + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512,512]; + for (int y = 0; y < (int)Constants.RegionSize; y++) + { + for (int x = 0; x < (int)Constants.RegionSize; x++) + { + resultarr2[y*2, x*2] = resultarr[y, x]; + + if (y < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2); + } + } + else + { + resultarr2[(y*2) + 1, x*2] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2); + } + } + else + { + resultarr2[y*2, (x*2) + 1] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize) + { + if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize)) + { + resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; + } + } + } + } + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) + { + if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) + { + m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0"); + resultarr2[y, x] = 0; + } + returnarr[i] = resultarr2[y, x]; + i++; + } + } + + return returnarr; + } + + #endregion + + public override void SetTerrain(float[] heightMap) + { + if (m_worldOffset != Vector3.Zero && m_parentScene != null) + { + if (m_parentScene is OdeScene) + { + ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset); + } + } + else + { + SetTerrain(heightMap, m_worldOffset); + } + } + + public void SetTerrain(float[] heightMap, Vector3 pOffset) + { + // this._heightmap[i] = (double)heightMap[i]; + // dbm (danx0r) -- creating a buffer zone of one extra sample all around + //_origheightmap = heightMap; + + float[] _heightmap; + + // zero out a heightmap array float array (single dimension [flattened])) + //if ((int)Constants.RegionSize == 256) + // _heightmap = new float[514 * 514]; + //else + + _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; + + uint heightmapWidth = Constants.RegionSize + 1; + uint heightmapHeight = Constants.RegionSize + 1; + + uint heightmapWidthSamples; + + uint heightmapHeightSamples; + + //if (((int)Constants.RegionSize) == 256) + //{ + // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2; + // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2; + // heightmapWidth++; + // heightmapHeight++; + //} + //else + //{ + + heightmapWidthSamples = (uint)Constants.RegionSize + 1; + heightmapHeightSamples = (uint)Constants.RegionSize + 1; + //} + + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 0.2f; + const int wrap = 0; + + int regionsize = (int) Constants.RegionSize + 2; + //Double resolution + //if (((int)Constants.RegionSize) == 256) + // heightMap = ResizeTerrain512Interpolation(heightMap); + + + // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256) + // regionsize = 512; + + float hfmin = 2000; + float hfmax = -2000; + + for (int x = 0; x < heightmapWidthSamples; x++) + { + for (int y = 0; y < heightmapHeightSamples; y++) + { + int xx = Util.Clip(x - 1, 0, regionsize - 1); + int yy = Util.Clip(y - 1, 0, regionsize - 1); + + + float val= heightMap[yy * (int)Constants.RegionSize + xx]; + _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val; + + hfmin = (val < hfmin) ? val : hfmin; + hfmax = (val > hfmax) ? val : hfmax; + } + } + + + + + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + TerrainHeightFieldHeights.Remove(GroundGeom); + } + d.SpaceRemove(space, GroundGeom); + d.GeomDestroy(GroundGeom); + } + + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1, + (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale, + offset, thickness, wrap); + d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + GroundGeom = d.CreateHeightfield(space, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[GroundGeom] = "Terrain"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); + + q1 = q1 * q2; + //q1 = q1 * q3; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(GroundGeom, ref R); + d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)) - 1, (pOffset.Y + ((int)Constants.RegionSize * 0.5f)) - 1, 0); + IntPtr testGround = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out testGround)) + { + RegionTerrain.Remove(pOffset); + } + RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); + TerrainHeightFieldHeights.Add(GroundGeom,_heightmap); + + } + } + + public override void DeleteTerrain() + { + } + + public float GetWaterLevel() + { + return waterlevel; + } + + public override bool SupportsCombining() + { + return true; + } + + public override void UnCombine(PhysicsScene pScene) + { + IntPtr localGround = IntPtr.Zero; +// float[] localHeightfield; + bool proceed = false; + List geomDestroyList = new List(); + + lock (OdeLock) + { + if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) + { + foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) + { + if (geom == localGround) + { +// localHeightfield = TerrainHeightFieldHeights[geom]; + proceed = true; + } + else + { + geomDestroyList.Add(geom); + } + } + + if (proceed) + { + m_worldOffset = Vector3.Zero; + WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + m_parentScene = null; + + foreach (IntPtr g in geomDestroyList) + { + // removingHeightField needs to be done or the garbage collector will + // collect the terrain data before we tell ODE to destroy it causing + // memory corruption + if (TerrainHeightFieldHeights.ContainsKey(g)) + { +// float[] removingHeightField = TerrainHeightFieldHeights[g]; + TerrainHeightFieldHeights.Remove(g); + + if (RegionTerrain.ContainsKey(g)) + { + RegionTerrain.Remove(g); + } + + d.GeomDestroy(g); + //removingHeightField = new float[0]; + } + } + + } + else + { + m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); + + } + } + } + } + + public override void SetWaterLevel(float baseheight) + { + waterlevel = baseheight; + randomizeWater(waterlevel); + } + + public void randomizeWater(float baseheight) + { + const uint heightmapWidth = m_regionWidth + 2; + const uint heightmapHeight = m_regionHeight + 2; + const uint heightmapWidthSamples = m_regionWidth + 2; + const uint heightmapHeightSamples = m_regionHeight + 2; + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 2.9f; + const int wrap = 0; + + for (int i = 0; i < (258 * 258); i++) + { + _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); + // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); + } + + lock (OdeLock) + { + if (WaterGeom != IntPtr.Zero) + { + d.SpaceRemove(space, WaterGeom); + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); + d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); + WaterGeom = d.CreateHeightfield(space, HeightmapData, 1); + if (WaterGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); + d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[WaterGeom] = "Water"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); + + q1 = q1 * q2; + //q1 = q1 * q3; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(WaterGeom, ref R); + d.GeomSetPosition(WaterGeom, 128, 128, 0); + + } + + } + + public override void Dispose() + { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + + lock (OdeLock) + { + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + RemovePrim(prm); + } + } + + //foreach (OdeCharacter act in _characters) + //{ + //RemoveAvatar(act); + //} + d.WorldDestroy(world); + //d.CloseODE(); + } + } + public override Dictionary GetTopColliders() + { + Dictionary returncolliders = new Dictionary(); + int cnt = 0; + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + if (prm.CollisionScore > 0) + { + returncolliders.Add(prm.m_localID, prm.CollisionScore); + cnt++; + prm.CollisionScore = 0f; + if (cnt > 25) + { + break; + } + } + } + } + return returncolliders; + } + + public override bool SupportsRayCast() + { + return true; + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null) + { + m_rayCastManager.QueueRequest(position, direction, length, retMethod); + } + } + +#if USE_DRAWSTUFF + // Keyboard callback + public void command(int cmd) + { + IntPtr geom; + d.Mass mass; + d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f); + + + + Char ch = Char.ToLower((Char)cmd); + switch ((Char)ch) + { + case 'w': + try + { + Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); + + xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z; + ds.SetViewpoint(ref xyz, ref hpr); + } + catch (ArgumentException) + { hpr.X = 0; } + break; + + case 'a': + hpr.X++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + + case 's': + try + { + Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); + + xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z; + ds.SetViewpoint(ref xyz, ref hpr); + } + catch (ArgumentException) + { hpr.X = 0; } + break; + case 'd': + hpr.X--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'r': + xyz.Z++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'f': + xyz.Z--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'e': + xyz.Y++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'q': + xyz.Y--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + } + } + + public void step(int pause) + { + + ds.SetColor(1.0f, 1.0f, 0.0f); + ds.SetTexture(ds.Texture.Wood); + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + //IntPtr body = d.GeomGetBody(prm.prim_geom); + if (prm.prim_geom != IntPtr.Zero) + { + d.Vector3 pos; + d.GeomCopyPosition(prm.prim_geom, out pos); + //d.BodyCopyPosition(body, out pos); + + d.Matrix3 R; + d.GeomCopyRotation(prm.prim_geom, out R); + //d.BodyCopyRotation(body, out R); + + + d.Vector3 sides = new d.Vector3(); + sides.X = prm.Size.X; + sides.Y = prm.Size.Y; + sides.Z = prm.Size.Z; + + ds.DrawBox(ref pos, ref R, ref sides); + } + } + } + ds.SetColor(1.0f, 0.0f, 0.0f); + lock (_characters) + { + foreach (OdeCharacter chr in _characters) + { + if (chr.Shell != IntPtr.Zero) + { + IntPtr body = d.GeomGetBody(chr.Shell); + + d.Vector3 pos; + d.GeomCopyPosition(chr.Shell, out pos); + //d.BodyCopyPosition(body, out pos); + + d.Matrix3 R; + d.GeomCopyRotation(chr.Shell, out R); + //d.BodyCopyRotation(body, out R); + + ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f); + d.Vector3 sides = new d.Vector3(); + sides.X = 0.5f; + sides.Y = 0.5f; + sides.Z = 0.5f; + + ds.DrawBox(ref pos, ref R, ref sides); + } + } + } + } + + public void start(int unused) + { + ds.SetViewpoint(ref xyz, ref hpr); + } +#endif + } +} diff --git a/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs new file mode 100644 index 0000000..69e2d03 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs @@ -0,0 +1,122 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using Nini.Config; +using NUnit.Framework; +using OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using log4net; +using System.Reflection; + +namespace OpenSim.Region.Physics.OdePlugin +{ + [TestFixture] + public class ODETestClass + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private OdePlugin cbt; + private PhysicsScene ps; + private IMeshingPlugin imp; + + [SetUp] + public void Initialize() + { + // Loading ODEPlugin + cbt = new OdePlugin(); + // Loading Zero Mesher + imp = new ZeroMesherPlugin(); + // Getting Physics Scene + ps = cbt.GetScene("test"); + // Initializing Physics Scene. + ps.Initialise(imp.GetMesher(),null); + float[] _heightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize]; + for (int i = 0; i < ((int)Constants.RegionSize * (int)Constants.RegionSize); i++) + { + _heightmap[i] = 21f; + } + ps.SetTerrain(_heightmap); + } + + [TearDown] + public void Terminate() + { + ps.DeleteTerrain(); + ps.Dispose(); + + } + + [Test] + public void CreateAndDropPhysicalCube() + { + PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox(); + Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f); + Vector3 size = new Vector3(0.5f, 0.5f, 0.5f); + Quaternion rot = Quaternion.Identity; + PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true); + OdePrim oprim = (OdePrim)prim; + OdeScene pscene = (OdeScene) ps; + + Assert.That(oprim.m_taintadd); + + prim.LocalID = 5; + + for (int i = 0; i < 58; i++) + { + ps.Simulate(0.133f); + + Assert.That(oprim.prim_geom != (IntPtr)0); + + Assert.That(oprim.m_targetSpace != (IntPtr)0); + + //Assert.That(oprim.m_targetSpace == pscene.space); + m_log.Info("TargetSpace: " + oprim.m_targetSpace + " - SceneMainSpace: " + pscene.space); + + Assert.That(!oprim.m_taintadd); + m_log.Info("Prim Position (" + oprim.m_localID + "): " + prim.Position.ToString()); + + // Make sure we're above the ground + //Assert.That(prim.Position.Z > 20f); + //m_log.Info("PrimCollisionScore (" + oprim.m_localID + "): " + oprim.m_collisionscore); + + // Make sure we've got a Body + Assert.That(oprim.Body != (IntPtr)0); + //m_log.Info( + } + + // Make sure we're not somewhere above the ground + Assert.That(prim.Position.Z < 21.5f); + + ps.RemovePrim(prim); + Assert.That(oprim.m_taintremove); + ps.Simulate(0.133f); + Assert.That(oprim.Body == (IntPtr)0); + } + } +} diff --git a/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs new file mode 100644 index 0000000..87ca446 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs @@ -0,0 +1,98 @@ +/* + * Copyright ODE + * Ode.NET - .NET bindings for ODE + * Jason Perkins (starkos@industriousone.com) + * Licensed under the New BSD + * Part of the OpenDynamicsEngine +Open Dynamics Engine +Copyright (c) 2001-2007, Russell L. Smith. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +Neither the names of ODE's copyright owner nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +using System; +using System.Runtime.InteropServices; +using Ode.NET; + +namespace Drawstuff.NET +{ +#if dDOUBLE + using dReal = System.Double; +#else + using dReal = System.Single; +#endif + + public static class ds + { + public const int VERSION = 2; + + public enum Texture + { + None, + Wood + } + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void CallbackFunction(int arg); + + [StructLayout(LayoutKind.Sequential)] + public struct Functions + { + public int version; + public CallbackFunction start; + public CallbackFunction step; + public CallbackFunction command; + public CallbackFunction stop; + public string path_to_textures; + } + + [DllImport("drawstuff", EntryPoint = "dsDrawBox")] + public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides); + + [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")] + public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius); + + [DllImport("drawstuff", EntryPoint = "dsDrawConvex")] + public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("drawstuff", EntryPoint = "dsSetColor")] + public static extern void SetColor(float red, float green, float blue); + + [DllImport("drawstuff", EntryPoint = "dsSetTexture")] + public static extern void SetTexture(Texture texture); + + [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")] + public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr); + + [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")] + public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn); + } +} diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs index b99baa2..06ed8fb 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -1197,26 +1197,13 @@ namespace OpenSim.Region.Physics.OdePlugin public override PIDHoverType PIDHoverType { set { return; } } public override float PIDHoverTau { set { return; } } - public override Quaternion APIDTarget - { - set { return; } - } + public override Quaternion APIDTarget{ set { return; } } - public override bool APIDActive - { - set { return; } - } + public override bool APIDActive{ set { return; } } - public override float APIDStrength - { - set { return; } - } - - public override float APIDDamping - { - set { return; } - } + public override float APIDStrength{ set { return; } } + public override float APIDDamping{ set { return; } } public override void SubscribeEvents(int ms) diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs index 78b15be..39cdc0f 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs @@ -23,19 +23,6 @@ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - * */ /* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces @@ -133,7 +120,7 @@ namespace OpenSim.Region.Physics.OdePlugin // private float m_VhoverEfficiency = 0f; private float m_VhoverTimescale = 0f; private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height - private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. @@ -492,7 +479,7 @@ namespace OpenSim.Region.Physics.OdePlugin Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object m_dir *= rotq; // apply obj rotation to velocity vector - // add Gravity and Buoyancy + // add Gravity andBuoyancy // KF: So far I have found no good method to combine a script-requested // .Z velocity and gravity. Therefore only 0g will used script-requested // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. @@ -574,7 +561,6 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body */ -//if(frcount == 0) Console.WriteLine("MoveAngular "); // Get what the body is doing, this includes 'external' influences d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); @@ -650,7 +636,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Deflection section tba // Sum velocities - m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection + m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) { diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index 688be83..3eb3b28 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -21,18 +21,6 @@ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. */ /* @@ -93,12 +81,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_PIDTau; private float PID_D = 35f; private float PID_G = 25f; - private bool m_usePID = false; - - private Quaternion m_APIDTarget = new Quaternion(); - private float m_APIDStrength = 0.5f; - private float m_APIDDamping = 0.5f; - private bool m_useAPID = false; + private bool m_usePID; // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), // and are for non-VEHICLES only. @@ -199,9 +182,6 @@ namespace OpenSim.Region.Physics.OdePlugin private ODEDynamics m_vehicle; internal int m_material = (int)Material.Wood; - - private int frcount = 0; // Used to limit dynamics debug output to - public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) @@ -1581,14 +1561,9 @@ Console.WriteLine(" JointCreateFixed"); float fy = 0; float fz = 0; - frcount++; // used to limit debug comment output - if (frcount > 100) - frcount = 0; if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. { -//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type + - // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); if (m_vehicle.Type != Vehicle.TYPE_NONE) { // 'VEHICLES' are dealt with in ODEDynamics.cs @@ -1596,6 +1571,7 @@ Console.WriteLine(" JointCreateFixed"); } else { +//Console.WriteLine("Move " + m_primName); if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 // NON-'VEHICLES' are dealt with here if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) @@ -1617,18 +1593,21 @@ Console.WriteLine(" JointCreateFixed"); //m_log.Info(m_collisionFlags.ToString()); - //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. + //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up - // NB Prims in ODE are no subject to global gravity - fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass + // gravityz multiplier = 1 - m_buoyancy + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; if (m_usePID) { -//if(frcount == 0) Console.WriteLine("PID " + m_primName); - // KF - this is for object MoveToTarget. - +//Console.WriteLine("PID " + m_primName); + // KF - this is for object move? eg. llSetPos() ? //if (!d.BodyIsEnabled(Body)) //d.BodySetForce(Body, 0f, 0f, 0f); + // If we're using the PID controller, then we have no gravity + //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... + fz = 0f; // no lock; for now it's only called from within Simulate() @@ -1763,7 +1742,7 @@ Console.WriteLine(" JointCreateFixed"); d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); d.BodySetLinearVel(Body, vel.X, vel.Y, 0); d.BodyAddForce(Body, 0, 0, fz); - //KF this prevents furthur motions return; + return; } else { @@ -1772,46 +1751,8 @@ Console.WriteLine(" JointCreateFixed"); // We're flying and colliding with something fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); } - } // end m_useHoverPID && !m_usePID - - if (m_useAPID) - { - // RotLookAt, apparently overrides all other rotation sources. Inputs: - // Quaternion m_APIDTarget - // float m_APIDStrength // From SL experiments, this is the time to get there - // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly - // Also in SL the mass of the object has no effect on time to get there. - // Factors: -//if(frcount == 0) Console.WriteLine("APID "); - // get present body rotation - float limit = 1.0f; - float scaler = 50f; // adjusts damping time - float RLAservo = 0f; - - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; - float diff_angle; - Vector3 diff_axis; - rot_diff.GetAxisAngle(out diff_axis, out diff_angle); - diff_axis.Normalize(); - if(diff_angle > 0.01f) // diff_angle is always +ve - { -// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); - Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); - rotforce = rotforce * rotq; - if(diff_angle > limit) diff_angle = limit; // cap the rotate rate -// RLAservo = timestep / m_APIDStrength * m_mass * scaler; - // rotforce = rotforce * RLAservo * diff_angle ; - // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); - RLAservo = timestep / m_APIDStrength * scaler; - rotforce = rotforce * RLAservo * diff_angle ; - d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); -//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); - } -//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); - } // end m_useAPID - + } + fx *= m_mass; fy *= m_mass; //fz *= m_mass; @@ -2673,7 +2614,7 @@ Console.WriteLine(" JointCreateFixed"); m_lastposition = _position; m_lastorientation = _orientation; - + l_position.X = vec.X; l_position.Y = vec.Y; l_position.Z = vec.Z; @@ -2681,10 +2622,6 @@ Console.WriteLine(" JointCreateFixed"); l_orientation.Y = ori.Y; l_orientation.Z = ori.Z; l_orientation.W = ori.W; - -// if(l_position.Y != m_lastposition.Y){ -// Console.WriteLine("UP&V {0} {1}", m_primName, l_position); -// } if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) { @@ -2885,17 +2822,20 @@ Console.WriteLine(" JointCreateFixed"); } public override bool PIDActive { set { m_usePID = value; } } public override float PIDTau { set { m_PIDTau = value; } } - - // For RotLookAt - public override Quaternion APIDTarget { set { m_APIDTarget = value; } } - public override bool APIDActive { set { m_useAPID = value; } } - public override float APIDStrength { set { m_APIDStrength = value; } } - public override float APIDDamping { set { m_APIDDamping = value; } } public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } public override bool PIDHoverActive { set { m_useHoverPID = value; } } public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + public override Quaternion APIDTarget{ set { return; } } + + public override bool APIDActive{ set { return; } } + + public override float APIDStrength{ set { return; } } + + public override float APIDDamping{ set { return; } } + private void createAMotor(Vector3 axis) { -- cgit v1.1 From e7439efc74a1cc0daedc51eb25ae66cd03db70b5 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 24 Dec 2009 19:19:44 -0500 Subject: Recover out-of-region objects during db load. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 983431d..0179240 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -215,6 +215,7 @@ namespace OpenSim.Region.Physics.OdePlugin parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); } + _position = pos; m_taintposition = pos; PID_D = parent_scene.bodyPIDD; @@ -254,7 +255,8 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene = parent_scene; m_targetSpace = (IntPtr)0; - if (pos.Z < 0) +// if (pos.Z < 0) + if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) m_isphysical = false; else { -- cgit v1.1 From 3f901d313bfd11070d5260f867c8a73c14f2d109 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 31 Dec 2009 16:07:36 -0500 Subject: Vehicle Linear parameter adjustments --- OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 40 ++++++++++++--- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 12 +++-- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 61 +++++++++++++---------- 3 files changed, 75 insertions(+), 38 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index 78b15be..ef2dccc 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs @@ -83,6 +83,12 @@ namespace OpenSim.Region.Physics.OdePlugin // private IntPtr m_jointGroup = IntPtr.Zero; // private IntPtr m_aMotor = IntPtr.Zero; + // Correction factors, to match Sl + private static float m_linearVelocityFactor = 0.9f; + private static float m_linearAttackFactor = 0.4f; + private static float m_linearDecayFactor = 0.5f; + private static float m_linearFrictionFactor = 1.2f; + // Vehicle properties private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind @@ -98,7 +104,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Linear properties private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time - private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL + private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL, for max limiting private Vector3 m_dir = Vector3.Zero; // velocity applied to body private Vector3 m_linearFrictionTimescale = Vector3.Zero; private float m_linearMotorDecayTimescale = 0; @@ -267,8 +273,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + pValue *= m_linearVelocityFactor; + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, decayed by time + m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting break; case Vehicle.LINEAR_MOTOR_OFFSET: // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); @@ -453,6 +460,17 @@ namespace OpenSim.Region.Physics.OdePlugin MoveAngular(pTimestep); }// end Step + internal void Halt() + { // Kill all motions, when non-physical + m_linearMotorDirection = Vector3.Zero; + m_linearMotorDirectionLASTSET = Vector3.Zero; + m_dir = Vector3.Zero; + m_lastLinearVelocityVector = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorVelocity = Vector3.Zero; + m_lastAngularVelocity = Vector3.Zero; + } + private void MoveLinear(float pTimestep, OdeScene _pParentScene) { if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant @@ -460,9 +478,15 @@ namespace OpenSim.Region.Physics.OdePlugin if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // add drive to body - Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); - m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? - + float linfactor = m_linearMotorTimescale/pTimestep; + // Linear accel + Vector3 addAmount1 = (m_linearMotorDirection/linfactor) * 0.8f; + // Differential accel + Vector3 addAmount2 = ((m_linearMotorDirection - m_lastLinearVelocityVector)/linfactor) * 1.6f; + // SL correction + Vector3 addAmount = (addAmount1 + addAmount2) * m_linearAttackFactor; + m_lastLinearVelocityVector += addAmount; // lastLinearVelocityVector is the current body velocity vector +//if(frcount == 0) Console.WriteLine("AL {0} + AD {1} AS{2} V {3}", addAmount1, addAmount2, addAmount, m_lastLinearVelocityVector); // This will work temporarily, but we really need to compare speed on an axis // KF: Limit body velocity to applied velocity? if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) @@ -475,7 +499,7 @@ namespace OpenSim.Region.Physics.OdePlugin // decay applied velocity Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); //Console.WriteLine("decay: " + decayfraction); - m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f; + m_linearMotorDirection -= m_linearMotorDirection * decayfraction * m_linearDecayFactor; //Console.WriteLine("actual: " + m_linearMotorDirection); } else @@ -560,7 +584,7 @@ namespace OpenSim.Region.Physics.OdePlugin // apply friction Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); - m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; + m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount * m_linearFrictionFactor; } // end MoveLinear() private void MoveAngular(float pTimestep) diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 0179240..6e6b44f 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -2296,11 +2296,15 @@ Console.WriteLine(" JointCreateFixed"); public override bool IsPhysical { get { return m_isphysical; } - set { + set + { m_isphysical = value; - if (!m_isphysical) // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - } + if (!m_isphysical) + { // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Halt(); + } + } } public void setPrimForRemoval() diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index f48649e..60786d4 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -229,7 +229,7 @@ namespace OpenSim.Region.Physics.OdePlugin public int bodyFramesAutoDisable = 20; - + protected DateTime m_lastframe = DateTime.UtcNow; private float[] _watermap; private bool m_filterCollisions = true; @@ -2639,13 +2639,20 @@ namespace OpenSim.Region.Physics.OdePlugin { if (framecount >= int.MaxValue) framecount = 0; - //if (m_worldOffset != Vector3.Zero) // return 0; framecount++; - - float fps = 0; + + DateTime now = DateTime.UtcNow; + TimeSpan SinceLastFrame = now - m_lastframe; + m_lastframe = now; + float realtime = (float)SinceLastFrame.TotalSeconds; +// Console.WriteLine("ts={0} rt={1}", timeStep, realtime); + timeStep = realtime; + + // float fps = 1.0f / realtime; + float fps = 0.0f; // number of ODE steps in this Simulate step //m_log.Info(timeStep.ToString()); step_time += timeStep; @@ -2691,11 +2698,11 @@ namespace OpenSim.Region.Physics.OdePlugin // Figure out the Frames Per Second we're going at. //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size - fps = (step_time / ODE_STEPSIZE) * 1000; + // fps = (step_time / ODE_STEPSIZE) * 1000; // HACK: Using a time dilation of 1.0 to debug rubberbanding issues //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f); - step_time = 0.09375f; + // step_time = 0.09375f; while (step_time > 0.0f) { @@ -2716,7 +2723,7 @@ namespace OpenSim.Region.Physics.OdePlugin foreach (OdeCharacter character in _taintedActors) { - character.ProcessTaints(timeStep); + character.ProcessTaints(ODE_STEPSIZE); processedtaints = true; //character.m_collisionscore = 0; @@ -2725,7 +2732,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (processedtaints) _taintedActors.Clear(); } - } + } // end lock _taintedActors // Modify other objects in the scene. processedtaints = false; @@ -2742,7 +2749,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { //Console.WriteLine("Simulate calls ProcessTaints"); - prim.ProcessTaints(timeStep); + prim.ProcessTaints(ODE_STEPSIZE); } processedtaints = true; prim.m_collisionscore = 0; @@ -2767,7 +2774,8 @@ namespace OpenSim.Region.Physics.OdePlugin foreach (PhysicsJoint joint in pendingJoints) { //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams); - string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries); + string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), + System.StringSplitOptions.RemoveEmptyEntries); List jointBodies = new List(); bool allJointBodiesAreReady = true; foreach (string jointParam in jointParams) @@ -2934,13 +2942,13 @@ namespace OpenSim.Region.Physics.OdePlugin //DoJointErrorMessage(successfullyProcessedJoint, "done"); } } - } + } // end SupportsNINJAJoints if (processedtaints) //Console.WriteLine("Simulate calls Clear of _taintedPrim list"); - _taintedPrimH.Clear(); + _taintedPrimH.Clear(); // ??? if this only ??? _taintedPrimL.Clear(); - } + } // end lock _taintedPrimLock // Move characters lock (_characters) @@ -2949,7 +2957,7 @@ namespace OpenSim.Region.Physics.OdePlugin foreach (OdeCharacter actor in _characters) { if (actor != null) - actor.Move(timeStep, defects); + actor.Move(ODE_STEPSIZE, defects); } if (0 != defects.Count) { @@ -2958,7 +2966,7 @@ namespace OpenSim.Region.Physics.OdePlugin RemoveCharacter(defect); } } - } + } // end lock _characters // Move other active objects lock (_activeprims) @@ -2966,9 +2974,9 @@ namespace OpenSim.Region.Physics.OdePlugin foreach (OdePrim prim in _activeprims) { prim.m_collisionscore = 0; - prim.Move(timeStep); + prim.Move(ODE_STEPSIZE); } - } + } // end lock _activeprims //if ((framecount % m_randomizeWater) == 0) // randomizeWater(waterlevel); @@ -2976,7 +2984,7 @@ namespace OpenSim.Region.Physics.OdePlugin //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests(); m_rayCastManager.ProcessQueuedRequests(); - collision_optimized(timeStep); + collision_optimized(ODE_STEPSIZE); lock (_collisionEventPrim) { @@ -2998,7 +3006,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; } } - } + } // end lock _collisionEventPrim //if (m_global_contactcount > 5) //{ @@ -3009,8 +3017,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldQuickStep(world, ODE_STEPSIZE); d.JointGroupEmpty(contactgroup); + fps++; //ode.dunlock(world); - } + } // end try catch (Exception e) { m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); @@ -3025,7 +3034,7 @@ namespace OpenSim.Region.Physics.OdePlugin //fps = 0; //} //} - } + } // end while (step_time > 0.0f) lock (_characters) { @@ -3090,7 +3099,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } } - } + } // end lock _activeprims //DumpJointInfo(); @@ -3111,10 +3120,10 @@ namespace OpenSim.Region.Physics.OdePlugin } d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); } - } - - return fps; - } + } // end lock OdeLock + + return fps * 1000.0f; //NB This is a FRAME COUNT, not a time! AND is divide by 1000 in SimStatusReporter! + } // end Simulate public override void GetResults() { -- cgit v1.1 From 66692f90e3b70ccdec8c148342578cd54acc4406 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 21 Jan 2010 14:39:11 -0500 Subject: ChODE Object Linear Motion update --- OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 187 ++++++++++++---------- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 2 +- 2 files changed, 102 insertions(+), 87 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index ef2dccc..9e145ec 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs @@ -82,13 +82,6 @@ namespace OpenSim.Region.Physics.OdePlugin private IntPtr m_body = IntPtr.Zero; // private IntPtr m_jointGroup = IntPtr.Zero; // private IntPtr m_aMotor = IntPtr.Zero; - - // Correction factors, to match Sl - private static float m_linearVelocityFactor = 0.9f; - private static float m_linearAttackFactor = 0.4f; - private static float m_linearDecayFactor = 0.5f; - private static float m_linearFrictionFactor = 1.2f; - // Vehicle properties private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind @@ -103,15 +96,15 @@ namespace OpenSim.Region.Physics.OdePlugin // LIMIT_ROLL_ONLY // Linear properties - private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time - private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL, for max limiting - private Vector3 m_dir = Vector3.Zero; // velocity applied to body - private Vector3 m_linearFrictionTimescale = Vector3.Zero; - private float m_linearMotorDecayTimescale = 0; - private float m_linearMotorTimescale = 0; - private Vector3 m_lastLinearVelocityVector = Vector3.Zero; - // private bool m_LinearMotorSetLastFrame = false; - // private Vector3 m_linearMotorOffset = Vector3.Zero; + private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity + //requested by LSL + private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL + private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL + private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL + + private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor + private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity + private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity //Angular properties private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor @@ -241,7 +234,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; case Vehicle.LINEAR_MOTOR_DIRECTION: m_linearMotorDirection = new Vector3(pValue, pValue, pValue); - m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + UpdateLinDecay(); break; case Vehicle.LINEAR_MOTOR_OFFSET: // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); @@ -273,9 +266,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.LINEAR_MOTOR_DIRECTION: - pValue *= m_linearVelocityFactor; - m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, decayed by time - m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting + UpdateLinDecay(); break; case Vehicle.LINEAR_MOTOR_OFFSET: // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); @@ -304,7 +296,7 @@ namespace OpenSim.Region.Physics.OdePlugin case Vehicle.TYPE_SLED: m_linearFrictionTimescale = new Vector3(30, 1, 1000); m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorDirection = Vector3.Zero; +// m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1000; m_linearMotorDecayTimescale = 120; m_angularMotorDirection = Vector3.Zero; @@ -330,7 +322,7 @@ namespace OpenSim.Region.Physics.OdePlugin case Vehicle.TYPE_CAR: m_linearFrictionTimescale = new Vector3(100, 2, 1000); m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorDirection = Vector3.Zero; +// m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -357,7 +349,7 @@ namespace OpenSim.Region.Physics.OdePlugin case Vehicle.TYPE_BOAT: m_linearFrictionTimescale = new Vector3(10, 3, 2); m_angularFrictionTimescale = new Vector3(10,10,10); - m_linearMotorDirection = Vector3.Zero; +// m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 5; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -385,7 +377,7 @@ namespace OpenSim.Region.Physics.OdePlugin case Vehicle.TYPE_AIRPLANE: m_linearFrictionTimescale = new Vector3(200, 10, 5); m_angularFrictionTimescale = new Vector3(20, 20, 20); - m_linearMotorDirection = Vector3.Zero; +// m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 2; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -412,7 +404,6 @@ namespace OpenSim.Region.Physics.OdePlugin case Vehicle.TYPE_BALLOON: m_linearFrictionTimescale = new Vector3(5, 5, 5); m_angularFrictionTimescale = new Vector3(10, 10, 10); - m_linearMotorDirection = Vector3.Zero; m_linearMotorTimescale = 5; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -453,7 +444,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) return; frcount++; // used to limit debug comment output - if (frcount > 100) + if (frcount > 24) frcount = 0; MoveLinear(pTimestep, pParentScene); @@ -463,63 +454,90 @@ namespace OpenSim.Region.Physics.OdePlugin internal void Halt() { // Kill all motions, when non-physical m_linearMotorDirection = Vector3.Zero; - m_linearMotorDirectionLASTSET = Vector3.Zero; - m_dir = Vector3.Zero; - m_lastLinearVelocityVector = Vector3.Zero; + m_lLinMotorDVel = Vector3.Zero; + m_lLinObjectVel = Vector3.Zero; + m_wLinObjectVel = Vector3.Zero; m_angularMotorDirection = Vector3.Zero; m_angularMotorVelocity = Vector3.Zero; m_lastAngularVelocity = Vector3.Zero; } + + private void UpdateLinDecay() + { + if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; + if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; + if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; + } // else let the motor decay on its own private void MoveLinear(float pTimestep, OdeScene _pParentScene) { - if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + Vector3 acceleration = new Vector3(0f, 0f, 0f); - // add drive to body - float linfactor = m_linearMotorTimescale/pTimestep; - // Linear accel - Vector3 addAmount1 = (m_linearMotorDirection/linfactor) * 0.8f; - // Differential accel - Vector3 addAmount2 = ((m_linearMotorDirection - m_lastLinearVelocityVector)/linfactor) * 1.6f; - // SL correction - Vector3 addAmount = (addAmount1 + addAmount2) * m_linearAttackFactor; - m_lastLinearVelocityVector += addAmount; // lastLinearVelocityVector is the current body velocity vector -//if(frcount == 0) Console.WriteLine("AL {0} + AD {1} AS{2} V {3}", addAmount1, addAmount2, addAmount, m_lastLinearVelocityVector); - // This will work temporarily, but we really need to compare speed on an axis - // KF: Limit body velocity to applied velocity? - if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) - m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; - if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) - m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; - if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) - m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; - - // decay applied velocity - Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); - //Console.WriteLine("decay: " + decayfraction); - m_linearMotorDirection -= m_linearMotorDirection * decayfraction * m_linearDecayFactor; - //Console.WriteLine("actual: " + m_linearMotorDirection); - } - else - { // requested is not significant - // if what remains of applied is small, zero it. - if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) - m_lastLinearVelocityVector = Vector3.Zero; - } - - - // convert requested object velocity to world-referenced vector - m_dir = m_lastLinearVelocityVector; d.Quaternion rot = d.BodyGetQuaternion(Body); Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - m_dir *= rotq; // apply obj rotation to velocity vector + Quaternion irotq = Quaternion.Inverse(rotq); + d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame + Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); + acceleration = vel_now - m_wLinObjectVel; + m_lLinObjectVel = vel_now * irotq; + + if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate + { + if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) + { + float decayfactor = m_linearMotorDecayTimescale/pTimestep; + Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); + m_lLinMotorDVel -= decayAmount; + } + else + { + float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); + Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * pTimestep; + m_lLinMotorDVel -= decel; + } + if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_lLinMotorDVel = Vector3.Zero; + } + else + { + if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; + if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; + if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; + } + } - // add Gravity and Buoyancy - // KF: So far I have found no good method to combine a script-requested - // .Z velocity and gravity. Therefore only 0g will used script-requested - // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. + if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + if (m_linearMotorTimescale < 300.0f) + { + Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; + float linfactor = m_linearMotorTimescale/pTimestep; + Vector3 attackAmount = (attack_error/linfactor) * 1.3f; + m_lLinObjectVel += attackAmount; + } + if (m_linearFrictionTimescale.X < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.X / pTimestep; + float fricX = m_lLinObjectVel.X / fricfactor; + m_lLinObjectVel.X -= fricX; + } + if (m_linearFrictionTimescale.Y < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Y / pTimestep; + float fricY = m_lLinObjectVel.Y / fricfactor; + m_lLinObjectVel.Y -= fricY; + } + if (m_linearFrictionTimescale.Z < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Z / pTimestep; + float fricZ = m_lLinObjectVel.Z / fricfactor; + m_lLinObjectVel.Z -= fricZ; + } + } + m_wLinObjectVel = m_lLinObjectVel * rotq; + // Add Gravity and Buoyancy Vector3 grav = Vector3.Zero; if(m_VehicleBuoyancy < 1.0f) { @@ -528,10 +546,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.Mass objMass; d.BodyGetMass(Body, out objMass); // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; - grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); - // Preserve the current Z velocity - d.Vector3 vel_now = d.BodyGetLinearVel(Body); - m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity + grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force } // else its 1.0, no gravity. // Check if hovering @@ -567,24 +582,24 @@ namespace OpenSim.Region.Physics.OdePlugin { d.Mass objMass; d.BodyGetMass(Body, out objMass); - m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); + m_wLinObjectVel.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); //KF: m_VhoverEfficiency is not yet implemented } else { - m_dir.Z = 0f; + m_wLinObjectVel.Z = 0f; } } - + else + { // not hovering, Gravity rules + m_wLinObjectVel.Z = vel_now.Z; +//if(frcount == 0) Console.WriteLine(" Z {0} a.Z {1}", m_wLinObjectVel.Z, acceleration.Z); + } // Apply velocity - d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); + d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z); // apply gravity force d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); - - - // apply friction - Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); - m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount * m_linearFrictionFactor; +//if(frcount == 0) Console.WriteLine("Grav {0}", grav); } // end MoveLinear() private void MoveAngular(float pTimestep) @@ -633,7 +648,7 @@ namespace OpenSim.Region.Physics.OdePlugin if(m_verticalAttractionTimescale < 300) { - float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep); + float VAservo = 0.0167f / (m_verticalAttractionTimescale * pTimestep); // get present body rotation d.Quaternion rot = d.BodyGetQuaternion(Body); Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 60786d4..deb6164 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -229,7 +229,7 @@ namespace OpenSim.Region.Physics.OdePlugin public int bodyFramesAutoDisable = 20; - protected DateTime m_lastframe = DateTime.UtcNow; + private DateTime m_lastframe = DateTime.UtcNow; private float[] _watermap; private bool m_filterCollisions = true; -- cgit v1.1 From 1abb70cc73c997c08a416fecf689b83453f853d0 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 21 Jan 2010 19:31:02 -0500 Subject: Add glue for llSetVehicleFlags(), llRemoveVehicleFlags(). ChODE: Add associated methods. --- .../Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs | 10 ++++++++++ .../BulletDotNETPlugin/BulletDotNETCharacter.cs | 10 ++++++++++ .../Physics/BulletDotNETPlugin/BulletDotNETPrim.cs | 10 ++++++++++ .../Region/Physics/BulletXPlugin/BulletXPlugin.cs | 10 ++++++++++ OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 10 ++++++++++ OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 10 ++++++++++ OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 12 +++++++++++- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 12 ++++++++++++ OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 10 ++++++++++ OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 10 ++++++++++ OpenSim/Region/Physics/POSPlugin/POSCharacter.cs | 10 ++++++++++ OpenSim/Region/Physics/POSPlugin/POSPrim.cs | 10 ++++++++++ OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs | 20 ++++++++++++++++++++ 13 files changed, 143 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs index 31366db..25b9099 100644 --- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs +++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs @@ -184,7 +184,17 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin { } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs index a3344dd..120d040 100644 --- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs @@ -361,7 +361,17 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin { } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs index 9603ea4..f430def 100644 --- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs @@ -396,7 +396,17 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin { //TODO: } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { //TODO: GhostObject diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs index d5d146e..9113ebe 100644 --- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs +++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs @@ -984,7 +984,17 @@ namespace OpenSim.Region.Physics.BulletXPlugin { } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index aa0acb7..2eb519f 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -733,7 +733,17 @@ namespace OpenSim.Region.Physics.OdePlugin { } + + public override void VehicleFlagsSet(int flags) + { + } + + public override void VehicleFlagsRemove(int flags) + { + + } + public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index 9e145ec..14d5caa 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs @@ -286,7 +286,17 @@ namespace OpenSim.Region.Physics.OdePlugin } }//end ProcessRotationVehicleParam + + internal void ProcessFlagsVehicleSet(int flags) + { + m_flags |= (VehicleFlag)flags; + } + internal void ProcessFlagsVehicleRemove(int flags) + { + m_flags &= ~((VehicleFlag)flags); + } + internal void ProcessTypeChange(Vehicle pType) { // Set Defaults For Type diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 6e6b44f..29a3dd9 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -2415,7 +2415,17 @@ Console.WriteLine(" JointCreateFixed"); { m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); } - + + public override void VehicleFlagsSet(int flags) + { + m_vehicle.ProcessFlagsVehicleSet(flags); + } + + public override void VehicleFlagsRemove(int flags) + { + m_vehicle.ProcessFlagsVehicleRemove(flags); + } + public override void SetVolumeDetect(int param) { lock (_parent_scene.OdeLock) diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 9c192ed..f43de48 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -208,6 +208,8 @@ namespace OpenSim.Region.Physics.Manager public abstract void VehicleFloatParam(int param, float value); public abstract void VehicleVectorParam(int param, Vector3 value); public abstract void VehicleRotationParam(int param, Quaternion rotation); + public abstract void VehicleFlagsSet(int flags); + public abstract void VehicleFlagsRemove(int flags); public abstract void SetVolumeDetect(int param); // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more @@ -351,7 +353,17 @@ namespace OpenSim.Region.Physics.Manager { } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs index a38fccc..b713142 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -733,7 +733,17 @@ namespace OpenSim.Region.Physics.OdePlugin { } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index 567fd0e..010d97f 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -2352,6 +2352,16 @@ Console.WriteLine(" JointCreateFixed"); { m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + + } public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs index 566b4e7..491e200 100644 --- a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs +++ b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs @@ -181,7 +181,17 @@ namespace OpenSim.Region.Physics.POSPlugin { } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs index edccf47..f8d49f9 100644 --- a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs +++ b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs @@ -138,7 +138,17 @@ namespace OpenSim.Region.Physics.POSPlugin { } + + public override void VehicleFlagsSet(int flags) + { + } + + public override void VehicleFlagsRemove(int flags) + { + + } + public override void SetVolumeDetect(int param) { diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs index 24eb6b1..e54065c 100644 --- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs +++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs @@ -370,7 +370,17 @@ namespace OpenSim.Region.Physics.PhysXPlugin { } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { @@ -774,7 +784,17 @@ namespace OpenSim.Region.Physics.PhysXPlugin { } + + public override void VehicleFlagsSet(int flags) + { + + } + + public override void VehicleFlagsRemove(int flags) + { + } + public override void SetVolumeDetect(int param) { -- cgit v1.1 From 8c2061029331e323b5c1c0414bbda417cb6c4674 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Tue, 2 Feb 2010 13:28:42 -0500 Subject: AngMotor update 1 --- OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 203 ++++++++++++++-------- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 19 +- 2 files changed, 143 insertions(+), 79 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index 14d5caa..c03e381 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs @@ -108,13 +108,14 @@ namespace OpenSim.Region.Physics.OdePlugin //Angular properties private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - private int m_angularMotorApply = 0; // application frame counter - private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity - private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate - private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate - private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate + + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL + private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL + + private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor +// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body - // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body //Deflection properties // private float m_angularDeflectionEfficiency = 0; @@ -227,7 +228,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; case Vehicle.ANGULAR_MOTOR_DIRECTION: m_angularMotorDirection = new Vector3(pValue, pValue, pValue); - m_angularMotorApply = 10; + UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); @@ -260,7 +261,7 @@ namespace OpenSim.Region.Physics.OdePlugin if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; - m_angularMotorApply = 10; + UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); @@ -310,6 +311,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorTimescale = 1000; m_linearMotorDecayTimescale = 120; m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; m_angularMotorTimescale = 1000; m_angularMotorDecayTimescale = 120; m_VhoverHeight = 0; @@ -336,6 +338,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorTimescale = 1; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; m_angularMotorTimescale = 1; m_angularMotorDecayTimescale = 0.8f; m_VhoverHeight = 0; @@ -363,6 +366,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorTimescale = 5; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; m_VhoverHeight = 0; @@ -391,6 +395,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorTimescale = 2; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; m_VhoverHeight = 0; @@ -417,6 +422,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorTimescale = 5; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; m_angularMotorTimescale = 6; m_angularMotorDecayTimescale = 10; m_VhoverHeight = 5; @@ -468,8 +474,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_lLinObjectVel = Vector3.Zero; m_wLinObjectVel = Vector3.Zero; m_angularMotorDirection = Vector3.Zero; - m_angularMotorVelocity = Vector3.Zero; - m_lastAngularVelocity = Vector3.Zero; + m_lastAngularVelocity = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; } private void UpdateLinDecay() @@ -611,54 +617,62 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); //if(frcount == 0) Console.WriteLine("Grav {0}", grav); } // end MoveLinear() + + private void UpdateAngDecay() + { + if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; + if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; + if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; + } // else let the motor decay on its own private void MoveAngular(float pTimestep) { /* - private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - private int m_angularMotorApply = 0; // application frame counter - private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) - private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate - private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate - private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate - private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL + private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL + + private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor + private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body */ //if(frcount == 0) Console.WriteLine("MoveAngular "); +//#### // Get what the body is doing, this includes 'external' influences - d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); - // Vector3 angularVelocity = Vector3.Zero; + d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); + Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); +//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); +// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel; +// Vector3 initavel = angObjectVel; + // Decay Angular Motor + if (m_angularMotorDecayTimescale < 300.0f) + { + float decayfactor = m_angularMotorDecayTimescale/pTimestep; + Vector3 decayAmount = (m_angularMotorDVel/decayfactor); + m_angularMotorDVel -= decayAmount; - if (m_angularMotorApply > 0) - { - // ramp up to new value - // current velocity += error / ( time to get there / step interval ) - // requested speed - last motor speed - m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); - m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); - m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); - - m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected - // velocity may still be acheived. - } - else - { - // no motor recently applied, keep the body velocity - /* m_angularMotorVelocity.X = angularVelocity.X; - m_angularMotorVelocity.Y = angularVelocity.Y; - m_angularMotorVelocity.Z = angularVelocity.Z; */ - - // and decay the velocity - m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); - } // end motor section - + if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_angularMotorDVel = Vector3.Zero; + } + else + { + if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; + if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; + if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; + } + } // end decay angular motor +//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); +//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); // Vertical attractor section Vector3 vertattr = Vector3.Zero; if(m_verticalAttractionTimescale < 300) { - float VAservo = 0.0167f / (m_verticalAttractionTimescale * pTimestep); + float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep); // get present body rotation d.Quaternion rot = d.BodyGetQuaternion(Body); Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); @@ -670,38 +684,88 @@ namespace OpenSim.Region.Physics.OdePlugin // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) - { + { // Defelction from vertical exceeds 90-degrees. This method will ensure stable return to + // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. +//Console.WriteLine("InvertFlip"); verterr.X = 2.0f - verterr.X; verterr.Y = 2.0f - verterr.Y; } - // Error is 0 (no error) to +/- 2 (max error) - // scale it by VAservo - verterr = verterr * VAservo; + verterr *= 0.5f; + // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) + + if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) + { +//if(frcount == 0) + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = - verterr.X; + vertattr.Z = 0f; //if(frcount == 0) Console.WriteLine("VAerr=" + verterr); - - // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so - // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. - vertattr.X = verterr.Y; - vertattr.Y = - verterr.X; - vertattr.Z = 0f; - - // scaling appears better usingsquare-law - float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); - vertattr.X += bounce * angularVelocity.X; - vertattr.Y += bounce * angularVelocity.Y; - + + // scaling appears better usingsquare-law + float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; + float bounce = 1.0f - damped; + // 0 = crit damp, 1 = bouncy + float oavz = angObjectVel.Z; // retain z velocity + angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy + angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); // damped, good @ < 90. + angObjectVel.Z = oavz; +//if(frcount == 0) Console.WriteLine("VA+"); +//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); + } + else + { + // else error is very small + angObjectVel.X = 0f; + angObjectVel.Y = 0f; +//if(frcount == 0) Console.WriteLine("VA0"); + } } // else vertical attractor is off +//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); + + if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) + { // if motor or object have motion + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + + if (m_angularMotorTimescale < 300.0f) + { + Vector3 attack_error = m_angularMotorDVel - angObjectVel; + float angfactor = m_angularMotorTimescale/pTimestep; + Vector3 attackAmount = (attack_error/angfactor); + angObjectVel += attackAmount; +//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); +//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); + } + + if (m_angularFrictionTimescale.X < 300.0f) + { + float fricfactor = m_angularFrictionTimescale.X / pTimestep; + angObjectVel.X -= angObjectVel.X / fricfactor; + } + if (m_angularFrictionTimescale.Y < 300.0f) + { + float fricfactor = m_angularFrictionTimescale.Y / pTimestep; + angObjectVel.Y -= angObjectVel.Y / fricfactor; + } + if (m_angularFrictionTimescale.Z < 300.0f) + { + float fricfactor = m_angularFrictionTimescale.Z / pTimestep; + angObjectVel.Z -= angObjectVel.Z / fricfactor; + Console.WriteLine("z fric"); + } + } // else no signif. motion - // m_lastVertAttractor = vertattr; - +//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); // Bank section tba // Deflection section tba +//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); - // Sum velocities - m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection - - if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) + m_lastAngularVelocity = angObjectVel; +/* + if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f)) { if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); } @@ -709,13 +773,12 @@ namespace OpenSim.Region.Physics.OdePlugin { m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. } - - // apply friction - Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); - m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; - + */ // Apply to the body +// Vector3 aInc = m_lastAngularVelocity - initavel; +//if(frcount == 0) Console.WriteLine("Inc {0}", aInc); d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); +//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); } //end MoveAngular } diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 29a3dd9..8502aef 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -2593,14 +2593,14 @@ Console.WriteLine(" JointCreateFixed"); { get { - Vector3 pv = Vector3.Zero; +/* Vector3 pv = Vector3.Zero; if (_zeroFlag) return pv; m_lastUpdateSent = false; if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) return pv; - +*/ return m_rotationalVelocity; } set @@ -2827,14 +2827,15 @@ Console.WriteLine(" JointCreateFixed"); _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); - if (_velocity.ApproxEquals(pv, 0.5f)) - { - m_rotationalVelocity = pv; - } - else - { +// if (_velocity.ApproxEquals(pv, 0.5f)) ???? Disregard rotational vel if lin vel is < 0.5 ????? +// { +// m_rotationalVelocity = pv;/ + +// } +// else +// { m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); - } +// } //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); _orientation.X = ori.X; -- cgit v1.1 From 48134af6162249885740bad80a6b1bbb618bbc6e Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Wed, 3 Feb 2010 16:20:13 -0500 Subject: Motor angular decay fix. --- OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index c03e381..4eb3313 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs @@ -646,12 +646,15 @@ namespace OpenSim.Region.Physics.OdePlugin //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); // Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel; // Vector3 initavel = angObjectVel; - // Decay Angular Motor + // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. + float atk_decayfactor = 23.0f / (m_angularMotorTimescale * pTimestep); + m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; + // Decay Angular Motor 2. if (m_angularMotorDecayTimescale < 300.0f) { - float decayfactor = m_angularMotorDecayTimescale/pTimestep; - Vector3 decayAmount = (m_angularMotorDVel/decayfactor); - m_angularMotorDVel -= decayAmount; + float decayfactor = m_angularMotorDecayTimescale/pTimestep; // df = Dec / pts + Vector3 decayAmount = (m_angularMotorDVel/decayfactor); // v-da = v-Dvel / df = v-Dvel * pts / Dec + m_angularMotorDVel -= decayAmount; // v-Dvel = v-Dvel - (v-Dvel / df = v-Dvel * pts / Dec) if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) { -- cgit v1.1 From 9568718addfd8e5d4094a456127ca469987f1f7a Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Fri, 12 Feb 2010 15:56:21 -0500 Subject: Angular motor adjustments --- OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 48 ++++++++++++----------- 1 file changed, 26 insertions(+), 22 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index 4eb3313..55d6945 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs @@ -224,6 +224,8 @@ namespace OpenSim.Region.Physics.OdePlugin // These are vector properties but the engine lets you use a single float value to // set all of the components to the same value case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue > 30f) pValue = 30f; + if (pValue < 0.1f) pValue = 0.1f; m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: @@ -250,6 +252,12 @@ namespace OpenSim.Region.Physics.OdePlugin switch (pParam) { case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X > 30f) pValue.X = 30f; + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y > 30f) pValue.Y = 30f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z > 30f) pValue.Z = 30f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: @@ -306,7 +314,7 @@ namespace OpenSim.Region.Physics.OdePlugin { case Vehicle.TYPE_SLED: m_linearFrictionTimescale = new Vector3(30, 1, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_angularFrictionTimescale = new Vector3(30, 30, 30); // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1000; m_linearMotorDecayTimescale = 120; @@ -333,7 +341,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; case Vehicle.TYPE_CAR: m_linearFrictionTimescale = new Vector3(100, 2, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1; m_linearMotorDecayTimescale = 60; @@ -548,6 +556,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_linearFrictionTimescale.Z < 300.0f) { float fricfactor = m_linearFrictionTimescale.Z / pTimestep; +//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor); float fricZ = m_lLinObjectVel.Z / fricfactor; m_lLinObjectVel.Z -= fricZ; } @@ -639,7 +648,6 @@ namespace OpenSim.Region.Physics.OdePlugin */ //if(frcount == 0) Console.WriteLine("MoveAngular "); -//#### // Get what the body is doing, this includes 'external' influences d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); @@ -652,9 +660,18 @@ namespace OpenSim.Region.Physics.OdePlugin // Decay Angular Motor 2. if (m_angularMotorDecayTimescale < 300.0f) { - float decayfactor = m_angularMotorDecayTimescale/pTimestep; // df = Dec / pts - Vector3 decayAmount = (m_angularMotorDVel/decayfactor); // v-da = v-Dvel / df = v-Dvel * pts / Dec - m_angularMotorDVel -= decayAmount; // v-Dvel = v-Dvel - (v-Dvel / df = v-Dvel * pts / Dec) +//#### + if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) + { + float decayfactor = (m_angularMotorDecayTimescale)/pTimestep; + Vector3 decayAmount = (m_angularMotorDVel/decayfactor); + m_angularMotorDVel -= decayAmount; + } + else + { + Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * pTimestep / m_angularMotorDecayTimescale; + m_angularMotorDVel -= decel; + } if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) { @@ -743,22 +760,9 @@ namespace OpenSim.Region.Physics.OdePlugin //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); } - if (m_angularFrictionTimescale.X < 300.0f) - { - float fricfactor = m_angularFrictionTimescale.X / pTimestep; - angObjectVel.X -= angObjectVel.X / fricfactor; - } - if (m_angularFrictionTimescale.Y < 300.0f) - { - float fricfactor = m_angularFrictionTimescale.Y / pTimestep; - angObjectVel.Y -= angObjectVel.Y / fricfactor; - } - if (m_angularFrictionTimescale.Z < 300.0f) - { - float fricfactor = m_angularFrictionTimescale.Z / pTimestep; - angObjectVel.Z -= angObjectVel.Z / fricfactor; - Console.WriteLine("z fric"); - } + angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / pTimestep); + angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / pTimestep); + angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / pTimestep); } // else no signif. motion //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); -- cgit v1.1 From 87590491b52e8ca0211274c17a83482534f845f3 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 4 Mar 2010 16:33:26 -0500 Subject: Fix Physics angular reference frame. --- .../Physics/ChOdePlugin/ODEDynamics.c_comments | 630 --------------------- OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 33 +- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 76 ++- 3 files changed, 84 insertions(+), 655 deletions(-) delete mode 100644 OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments deleted file mode 100644 index 1060aa6..0000000 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments +++ /dev/null @@ -1,630 +0,0 @@ -/* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - * - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using log4net; -using OpenMetaverse; -using Ode.NET; -using OpenSim.Framework; -using OpenSim.Region.Physics.Manager; - -namespace OpenSim.Region.Physics.OdePlugin -{ - public class ODEDynamics - { - public Vehicle Type - { - get { return m_type; } - } - - public IntPtr Body - { - get { return m_body; } - } - - private int frcount = 0; // Used to limit dynamics debug output to - // every 100th frame - - // private OdeScene m_parentScene = null; - private IntPtr m_body = IntPtr.Zero; - private IntPtr m_jointGroup = IntPtr.Zero; - private IntPtr m_aMotor = IntPtr.Zero; - - - // Vehicle properties - private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind - // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier - private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: - // HOVER_TERRAIN_ONLY - // HOVER_GLOBAL_HEIGHT - // NO_DEFLECTION_UP - // HOVER_WATER_ONLY - // HOVER_UP_ONLY - // LIMIT_MOTOR_UP - // LIMIT_ROLL_ONLY - - // Linear properties - private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time - private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL - private Vector3 m_dir = Vector3.Zero; // velocity applied to body - private Vector3 m_linearFrictionTimescale = Vector3.Zero; - private float m_linearMotorDecayTimescale = 0; - private float m_linearMotorTimescale = 0; - private Vector3 m_lastLinearVelocityVector = Vector3.Zero; - // private bool m_LinearMotorSetLastFrame = false; - // private Vector3 m_linearMotorOffset = Vector3.Zero; - - //Angular properties - private Vector3 m_angularMotorDirection = Vector3.Zero; - private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; - private Vector3 m_angularFrictionTimescale = Vector3.Zero; - private float m_angularMotorDecayTimescale = 0; - private float m_angularMotorTimescale = 0; - private Vector3 m_lastAngularVelocityVector = Vector3.Zero; - - //Deflection properties - // private float m_angularDeflectionEfficiency = 0; - // private float m_angularDeflectionTimescale = 0; - // private float m_linearDeflectionEfficiency = 0; - // private float m_linearDeflectionTimescale = 0; - - //Banking properties - // private float m_bankingEfficiency = 0; - // private float m_bankingMix = 0; - // private float m_bankingTimescale = 0; - - //Hover and Buoyancy properties - private float m_VhoverHeight = 0f; - private float m_VhoverEfficiency = 0f; - private float m_VhoverTimescale = 0f; - private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height - private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. - // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) - // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. - // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. - - //Attractor properties - private float m_verticalAttractionEfficiency = 0; - private float m_verticalAttractionTimescale = 0; - - - - - - internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) - { - switch (pParam) - { - case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_angularDeflectionEfficiency = pValue; - break; - case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_angularDeflectionTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_angularMotorDecayTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_angularMotorTimescale = pValue; - break; - case Vehicle.BANKING_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingEfficiency = pValue; - break; - case Vehicle.BANKING_MIX: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingMix = pValue; - break; - case Vehicle.BANKING_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingTimescale = pValue; - break; - case Vehicle.BUOYANCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; - m_VehicleBuoyancy = pValue; - break; - case Vehicle.HOVER_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - m_VhoverEfficiency = pValue; - break; - case Vehicle.HOVER_HEIGHT: - m_VhoverHeight = pValue; - break; - case Vehicle.HOVER_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_VhoverTimescale = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_linearDeflectionEfficiency = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_linearDeflectionTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_linearMotorDecayTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_linearMotorTimescale = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: - if (pValue < 0.0f) pValue = 0.0f; - if (pValue > 1.0f) pValue = 1.0f; - m_verticalAttractionEfficiency = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_verticalAttractionTimescale = pValue; - break; - - // These are vector properties but the engine lets you use a single float value to - // set all of the components to the same value - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue, pValue, pValue); - m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue, pValue, pValue); - m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); - break; - - } - - }//end ProcessFloatVehicleParam - - internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) - { - switch (pParam) - { - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - } - - }//end ProcessVectorVehicleParam - - internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) - { - switch (pParam) - { - case Vehicle.REFERENCE_FRAME: - // m_referenceFrame = pValue; - break; - } - - }//end ProcessRotationVehicleParam - - internal void ProcessTypeChange(Vehicle pType) - { -Console.WriteLine("ProcessTypeChange to " + pType); - - // Set Defaults For Type - m_type = pType; - switch (pType) - { - case Vehicle.TYPE_SLED: - m_linearFrictionTimescale = new Vector3(30, 1, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 120; - m_VhoverHeight = 0; - m_VhoverEfficiency = 1; - m_VhoverTimescale = 10; - m_VehicleBuoyancy = 0; - // m_linearDeflectionEfficiency = 1; - // m_linearDeflectionTimescale = 1; - // m_angularDeflectionEfficiency = 1; - // m_angularDeflectionTimescale = 1000; - // m_bankingEfficiency = 0; - // m_bankingMix = 1; - // m_bankingTimescale = 10; - // m_referenceFrame = Quaternion.Identity; - m_flags &= - ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_CAR: - m_linearFrictionTimescale = new Vector3(100, 2, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 1; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 1; - m_angularMotorDecayTimescale = 0.8f; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - // // m_linearDeflectionEfficiency = 1; - // // m_linearDeflectionTimescale = 2; - // // m_angularDeflectionEfficiency = 0; - // m_angularDeflectionTimescale = 10; - m_verticalAttractionEfficiency = 1; - m_verticalAttractionTimescale = 10; - // m_bankingEfficiency = -0.2f; - // m_bankingMix = 1; - // m_bankingTimescale = 1; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_BOAT: - m_linearFrictionTimescale = new Vector3(10, 3, 2); - m_angularFrictionTimescale = new Vector3(10,10,10); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 2; - m_VehicleBuoyancy = 1; - // m_linearDeflectionEfficiency = 0.5f; - // m_linearDeflectionTimescale = 3; - // m_angularDeflectionEfficiency = 0.5f; - // m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 0.5f; - m_verticalAttractionTimescale = 5; - // m_bankingEfficiency = -0.3f; - // m_bankingMix = 0.8f; - // m_bankingTimescale = 1; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | - VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_AIRPLANE: - m_linearFrictionTimescale = new Vector3(200, 10, 5); - m_angularFrictionTimescale = new Vector3(20, 20, 20); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 2; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - // m_linearDeflectionEfficiency = 0.5f; - // m_linearDeflectionTimescale = 3; - // m_angularDeflectionEfficiency = 1; - // m_angularDeflectionTimescale = 2; - m_verticalAttractionEfficiency = 0.9f; - m_verticalAttractionTimescale = 2; - // m_bankingEfficiency = 1; - // m_bankingMix = 0.7f; - // m_bankingTimescale = 2; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); - break; - case Vehicle.TYPE_BALLOON: - m_linearFrictionTimescale = new Vector3(5, 5, 5); - m_angularFrictionTimescale = new Vector3(10, 10, 10); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 6; - m_angularMotorDecayTimescale = 10; - m_VhoverHeight = 5; - m_VhoverEfficiency = 0.8f; - m_VhoverTimescale = 10; - m_VehicleBuoyancy = 1; - // m_linearDeflectionEfficiency = 0; - // m_linearDeflectionTimescale = 5; - // m_angularDeflectionEfficiency = 0; - // m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 1; - m_verticalAttractionTimescale = 1000; - // m_bankingEfficiency = 0; - // m_bankingMix = 0.7f; - // m_bankingTimescale = 5; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - break; - - } - }//end SetDefaultsForType - - internal void Enable(IntPtr pBody, OdeScene pParentScene) - { -//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy); - if (m_type == Vehicle.TYPE_NONE) - return; - - m_body = pBody; - //KF: This used to set up the linear and angular joints - } - - internal void Step(float pTimestep, OdeScene pParentScene) - { - if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) - return; - frcount++; // used to limit debug comment output - if (frcount > 100) - frcount = 0; - - MoveLinear(pTimestep, pParentScene); - MoveAngular(pTimestep); - }// end Step - - private void MoveLinear(float pTimestep, OdeScene _pParentScene) - { - if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - - // add drive to body - Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); - m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? - - // This will work temporarily, but we really need to compare speed on an axis - // KF: Limit body velocity to applied velocity? - if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) - m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; - if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) - m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; - if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) - m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; - - // decay applied velocity - Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); - //Console.WriteLine("decay: " + decayfraction); - m_linearMotorDirection -= m_linearMotorDirection * decayfraction; - //Console.WriteLine("actual: " + m_linearMotorDirection); - } - else - { // requested is not significant - // if what remains of applied is small, zero it. - if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) - m_lastLinearVelocityVector = Vector3.Zero; - } - - - // convert requested object velocity to world-referenced vector - m_dir = m_lastLinearVelocityVector; - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - m_dir *= rotq; // apply obj rotation to velocity vector - - // add Gravity andBuoyancy - // KF: So far I have found no good method to combine a script-requested - // .Z velocity and gravity. Therefore only 0g will used script-requested - // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. - Vector3 grav = Vector3.Zero; - if(m_VehicleBuoyancy < 1.0f) - { - // There is some gravity, make a gravity force vector - // that is applied after object velocity. - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; - grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); - // Preserve the current Z velocity - d.Vector3 vel_now = d.BodyGetLinearVel(Body); - m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity - } // else its 1.0, no gravity. - - // Check if hovering - if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) - { - // We should hover, get the target height - d.Vector3 pos = d.BodyGetPosition(Body); - if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) - { - m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) - { - m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) - { - m_VhoverTargetHeight = m_VhoverHeight; - } - - if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) - { - // If body is aready heigher, use its height as target height - if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; - } - -// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped -// m_VhoverTimescale = 0f; // time to acheive height -// pTimestep is time since last frame,in secs - float herr0 = pos.Z - m_VhoverTargetHeight; -//if(frcount == 0) Console.WriteLine("herr0=" + herr0); - // Replace Vertical speed with correction figure if significant - if(Math.Abs(herr0) > 0.01f ) - { - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); - // m_VhoverEfficiency is not yet implemented - } - else - { - m_dir.Z = 0f; - } - } - - // Apply velocity - d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); -//if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z); - // apply gravity force - d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); -//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z); - - - // apply friction - Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); - m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; - } // end MoveLinear() - - private void MoveAngular(float pTimestep) - { - - // m_angularMotorDirection is the latest value from the script, and is decayed here - // m_angularMotorDirectionLASTSET is the latest value from the script - // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here - - if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - // ramp up to new value - Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); - m_lastAngularVelocityVector += (addAmount * 10f); -//if(frcount == 0) Console.WriteLine("add: " + addAmount); - - // limit applied value to what was set by script - // This will work temporarily, but we really need to compare speed on an axis - if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X)) - m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; - if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y)) - m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; - if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) - m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; - - // decay the requested value - Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); - //Console.WriteLine("decay: " + decayfraction); - m_angularMotorDirection -= m_angularMotorDirection * decayfraction; - //Console.WriteLine("actual: " + m_linearMotorDirection); - } - // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ? - - // Vertical attractor section - -// d.Mass objMass; -// d.BodyGetMass(Body, out objMass); -// float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); - float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); - // get present body rotation - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - // make a vector pointing up - Vector3 verterr = Vector3.Zero; - verterr.Z = 1.0f; - // rotate it to Body Angle - verterr = verterr * rotq; - // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. - // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go - // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. - if (verterr.Z < 0.0f) - { - verterr.X = 2.0f - verterr.X; - verterr.Y = 2.0f - verterr.Y; - } - // Error is 0 (no error) to +/- 2 (max error) - // scale it by servo - verterr = verterr * servo; - - // rotate to object frame - // verterr = verterr * rotq; - - // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so - // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. - m_lastAngularVelocityVector.X += verterr.Y; - m_lastAngularVelocityVector.Y -= verterr.X; -/* -if(frcount == 0) - { -// Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector); - Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}", - Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency)); - } - */ - d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z); - // apply friction - Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); - m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; - - } //end MoveAngular - } -} diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index 55d6945..b3b09e6 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs @@ -116,6 +116,8 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + + private Vector3 m_angularLock = Vector3.One; //Deflection properties // private float m_angularDeflectionEfficiency = 0; @@ -296,6 +298,11 @@ namespace OpenSim.Region.Physics.OdePlugin }//end ProcessRotationVehicleParam + internal void SetAngularLock(Vector3 pValue) + { + m_angularLock = pValue; + } + internal void ProcessFlagsVehicleSet(int flags) { m_flags |= (VehicleFlag)flags; @@ -649,8 +656,13 @@ namespace OpenSim.Region.Physics.OdePlugin //if(frcount == 0) Console.WriteLine("MoveAngular "); // Get what the body is doing, this includes 'external' influences + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion irotq = Quaternion.Inverse(rotq); d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); + angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation + //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); // Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel; // Vector3 initavel = angObjectVel; @@ -694,8 +706,8 @@ namespace OpenSim.Region.Physics.OdePlugin { float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep); // get present body rotation - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); +// d.Quaternion rot = d.BodyGetQuaternion(Body); +// Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // make a vector pointing up Vector3 verterr = Vector3.Zero; verterr.Z = 1.0f; @@ -706,7 +718,7 @@ namespace OpenSim.Region.Physics.OdePlugin // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. if (verterr.Z < 0.0f) - { // Defelction from vertical exceeds 90-degrees. This method will ensure stable return to + { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. //Console.WriteLine("InvertFlip"); verterr.X = 2.0f - verterr.X; @@ -781,9 +793,22 @@ namespace OpenSim.Region.Physics.OdePlugin m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. } */ +//if(frcount == 0) Console.WriteLine("angularLock {0}", m_angularLock); + + if (!m_angularLock.ApproxEquals(Vector3.One, 0.003f)) + { + if (m_angularLock.X == 0) + m_lastAngularVelocity.X = 0f; + if (m_angularLock.Y == 0) + m_lastAngularVelocity.Y = 0f; + if (m_angularLock.Z == 0) + m_lastAngularVelocity.Z = 0f; + } // Apply to the body // Vector3 aInc = m_lastAngularVelocity - initavel; -//if(frcount == 0) Console.WriteLine("Inc {0}", aInc); +//if(frcount == 0) Console.WriteLine("Inc {0}", aInc); + m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation + d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 8502aef..fb6cb55 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -85,8 +85,10 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_taintVelocity; private Vector3 m_taintTorque; private Quaternion m_taintrot; - private Vector3 m_angularlock = Vector3.One; - private Vector3 m_taintAngularLock = Vector3.One; + private Vector3 m_angularlock = Vector3.One; // Current setting + private Vector3 m_taintAngularLock = Vector3.One; // Request from LSL + + private IntPtr Amotor = IntPtr.Zero; private Vector3 m_PIDTarget; @@ -405,10 +407,10 @@ namespace OpenSim.Region.Physics.OdePlugin m_disabled = false; // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) + /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) { createAMotor(m_angularlock); - } + } */ if (m_vehicle.Type != Vehicle.TYPE_NONE) { m_vehicle.Enable(Body, _parent_scene); @@ -958,8 +960,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); - + + // ##* if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) +//Console.WriteLine("ALchange req {0} is {1}", m_taintAngularLock, m_angularlock); changeAngularLock(timestep); } @@ -970,15 +974,21 @@ namespace OpenSim.Region.Physics.OdePlugin } - private void changeAngularLock(float timestep) + private void changeAngularLock(float timestep) // ##* { // do we have a Physical object? - if (Body != IntPtr.Zero) - { +// if (Body != IntPtr.Zero) +// { //Check that we have a Parent //If we have a parent then we're not authorative here if (_parent == null) { +//Console.WriteLine("Alock changed to {0}", m_taintAngularLock); + m_angularlock = m_taintAngularLock; + m_vehicle.SetAngularLock(m_angularlock); + + +/* if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) { //d.BodySetFiniteRotationMode(Body, 0); @@ -997,7 +1007,9 @@ namespace OpenSim.Region.Physics.OdePlugin } // Store this for later in case we get turned into a separate body m_angularlock = m_taintAngularLock; - + m_vehicle.SetAngularLock(m_angularlock); + } */ + } } private void changelink(float timestep) @@ -1140,10 +1152,10 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_disabled = false; // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) + /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) { prm.createAMotor(m_angularlock); - } + } */ prm.Body = Body; _parent_scene.addActivePrim(prm); } @@ -1183,10 +1195,10 @@ namespace OpenSim.Region.Physics.OdePlugin m_disabled = false; // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) + /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) { createAMotor(m_angularlock); - } + } */ d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene); _parent_scene.addActivePrim(this); @@ -1600,14 +1612,17 @@ Console.WriteLine(" JointCreateFixed"); { if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 // NON-'VEHICLES' are dealt with here - if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) + // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked. + +// NB this may be wrong - may lock global axis! Should be LOCAL axis! + if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f)) { d.Vector3 avel2 = d.BodyGetAngularVel(Body); - if (m_angularlock.X == 1) + if (m_angularlock.X == 0) avel2.X = 0; - if (m_angularlock.Y == 1) + if (m_angularlock.Y == 0) avel2.Y = 0; - if (m_angularlock.Z == 1) + if (m_angularlock.Z == 0) avel2.Z = 0; d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); } @@ -1808,6 +1823,17 @@ Console.WriteLine(" JointCreateFixed"); // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); RLAservo = timestep / m_APIDStrength * scaler; rotforce = rotforce * RLAservo * diff_angle ; + + if (m_angularlock.X == 0) + rotforce.X = 0; + if (m_angularlock.Y == 0) + rotforce.Y = 0; + if (m_angularlock.Z == 0) + rotforce.Z = 0; + + + + d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); } @@ -1878,11 +1904,11 @@ Console.WriteLine(" JointCreateFixed"); { // KF: If this is a root prim do BodySet d.BodySetQuaternion(Body, ref myrot); - if (m_isphysical) + /* ### if (m_isphysical) { if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) createAMotor(m_angularlock); - } + } */ } else { @@ -2508,7 +2534,14 @@ Console.WriteLine(" JointCreateFixed"); } } } - +/* + public Vector3 AngularLock + { + get { return m_angularlock; } + set { } + } + +*/ public override float CollisionScore { get { return m_collisionscore; } @@ -2914,8 +2947,9 @@ Console.WriteLine(" JointCreateFixed"); public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } public override float PIDHoverTau { set { m_PIDHoverTau = value; } } - private void createAMotor(Vector3 axis) + private void createAMotor(Vector3 axis) // ##* { +Console.WriteLine(" createAMotor called! ----------------------------"); if (Body == IntPtr.Zero) return; -- cgit v1.1 From f6f6ef1532517972b973d8a500818dcd50873352 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 11 Mar 2010 19:12:38 -0500 Subject: Dynamics Integration Part 1 --- OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 817 ----- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 3816 ++++++++++++--------- 2 files changed, 2256 insertions(+), 2377 deletions(-) delete mode 100644 OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs deleted file mode 100644 index b3b09e6..0000000 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ /dev/null @@ -1,817 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - * - */ - -/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - */ - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using log4net; -using OpenMetaverse; -using Ode.NET; -using OpenSim.Framework; -using OpenSim.Region.Physics.Manager; - -namespace OpenSim.Region.Physics.OdePlugin -{ - public class ODEDynamics - { - public Vehicle Type - { - get { return m_type; } - } - - public IntPtr Body - { - get { return m_body; } - } - - private int frcount = 0; // Used to limit dynamics debug output to - // every 100th frame - - // private OdeScene m_parentScene = null; - private IntPtr m_body = IntPtr.Zero; -// private IntPtr m_jointGroup = IntPtr.Zero; -// private IntPtr m_aMotor = IntPtr.Zero; - - // Vehicle properties - private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind - // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier - private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: - // HOVER_TERRAIN_ONLY - // HOVER_GLOBAL_HEIGHT - // NO_DEFLECTION_UP - // HOVER_WATER_ONLY - // HOVER_UP_ONLY - // LIMIT_MOTOR_UP - // LIMIT_ROLL_ONLY - - // Linear properties - private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity - //requested by LSL - private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL - private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL - private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL - - private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor - private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity - private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity - - //Angular properties - private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - - private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL - private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL - private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL - - private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor -// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity - private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body - - private Vector3 m_angularLock = Vector3.One; - - //Deflection properties - // private float m_angularDeflectionEfficiency = 0; - // private float m_angularDeflectionTimescale = 0; - // private float m_linearDeflectionEfficiency = 0; - // private float m_linearDeflectionTimescale = 0; - - //Banking properties - // private float m_bankingEfficiency = 0; - // private float m_bankingMix = 0; - // private float m_bankingTimescale = 0; - - //Hover and Buoyancy properties - private float m_VhoverHeight = 0f; -// private float m_VhoverEfficiency = 0f; - private float m_VhoverTimescale = 0f; - private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height - private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. - // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) - // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. - // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. - - //Attractor properties - private float m_verticalAttractionEfficiency = 1.0f; // damped - private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. - - - - - - internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) - { - switch (pParam) - { - case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_angularDeflectionEfficiency = pValue; - break; - case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_angularDeflectionTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_angularMotorDecayTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_angularMotorTimescale = pValue; - break; - case Vehicle.BANKING_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingEfficiency = pValue; - break; - case Vehicle.BANKING_MIX: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingMix = pValue; - break; - case Vehicle.BANKING_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingTimescale = pValue; - break; - case Vehicle.BUOYANCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; - m_VehicleBuoyancy = pValue; - break; -// case Vehicle.HOVER_EFFICIENCY: -// if (pValue < 0f) pValue = 0f; -// if (pValue > 1f) pValue = 1f; -// m_VhoverEfficiency = pValue; -// break; - case Vehicle.HOVER_HEIGHT: - m_VhoverHeight = pValue; - break; - case Vehicle.HOVER_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_VhoverTimescale = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_linearDeflectionEfficiency = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_linearDeflectionTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_linearMotorDecayTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_linearMotorTimescale = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: - if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable - if (pValue > 1.0f) pValue = 1.0f; - m_verticalAttractionEfficiency = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_verticalAttractionTimescale = pValue; - break; - - // These are vector properties but the engine lets you use a single float value to - // set all of the components to the same value - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue > 30f) pValue = 30f; - if (pValue < 0.1f) pValue = 0.1f; - m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue, pValue, pValue); - UpdateAngDecay(); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue, pValue, pValue); - UpdateLinDecay(); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); - break; - - } - - }//end ProcessFloatVehicleParam - - internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) - { - switch (pParam) - { - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue.X > 30f) pValue.X = 30f; - if (pValue.X < 0.1f) pValue.X = 0.1f; - if (pValue.Y > 30f) pValue.Y = 30f; - if (pValue.Y < 0.1f) pValue.Y = 0.1f; - if (pValue.Z > 30f) pValue.Z = 30f; - if (pValue.Z < 0.1f) pValue.Z = 0.1f; - m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - // Limit requested angular speed to 2 rps= 4 pi rads/sec - if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; - if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; - if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; - if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; - if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; - if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; - UpdateAngDecay(); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting - UpdateLinDecay(); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - } - - }//end ProcessVectorVehicleParam - - internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) - { - switch (pParam) - { - case Vehicle.REFERENCE_FRAME: - // m_referenceFrame = pValue; - break; - } - - }//end ProcessRotationVehicleParam - - internal void SetAngularLock(Vector3 pValue) - { - m_angularLock = pValue; - } - - internal void ProcessFlagsVehicleSet(int flags) - { - m_flags |= (VehicleFlag)flags; - } - - internal void ProcessFlagsVehicleRemove(int flags) - { - m_flags &= ~((VehicleFlag)flags); - } - - internal void ProcessTypeChange(Vehicle pType) - { - // Set Defaults For Type - m_type = pType; - switch (pType) - { - case Vehicle.TYPE_SLED: - m_linearFrictionTimescale = new Vector3(30, 1, 1000); - m_angularFrictionTimescale = new Vector3(30, 30, 30); -// m_lLinMotorVel = Vector3.Zero; - m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 120; - m_VhoverHeight = 0; -// m_VhoverEfficiency = 1; - m_VhoverTimescale = 10; - m_VehicleBuoyancy = 0; - // m_linearDeflectionEfficiency = 1; - // m_linearDeflectionTimescale = 1; - // m_angularDeflectionEfficiency = 1; - // m_angularDeflectionTimescale = 1000; - // m_bankingEfficiency = 0; - // m_bankingMix = 1; - // m_bankingTimescale = 10; - // m_referenceFrame = Quaternion.Identity; - m_flags &= - ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_CAR: - m_linearFrictionTimescale = new Vector3(100, 2, 1000); - m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. -// m_lLinMotorVel = Vector3.Zero; - m_linearMotorTimescale = 1; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - m_angularMotorTimescale = 1; - m_angularMotorDecayTimescale = 0.8f; - m_VhoverHeight = 0; -// m_VhoverEfficiency = 0; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - // // m_linearDeflectionEfficiency = 1; - // // m_linearDeflectionTimescale = 2; - // // m_angularDeflectionEfficiency = 0; - // m_angularDeflectionTimescale = 10; - m_verticalAttractionEfficiency = 1f; - m_verticalAttractionTimescale = 10f; - // m_bankingEfficiency = -0.2f; - // m_bankingMix = 1; - // m_bankingTimescale = 1; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_BOAT: - m_linearFrictionTimescale = new Vector3(10, 3, 2); - m_angularFrictionTimescale = new Vector3(10,10,10); -// m_lLinMotorVel = Vector3.Zero; - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4; - m_VhoverHeight = 0; -// m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 2; - m_VehicleBuoyancy = 1; - // m_linearDeflectionEfficiency = 0.5f; - // m_linearDeflectionTimescale = 3; - // m_angularDeflectionEfficiency = 0.5f; - // m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 0.5f; - m_verticalAttractionTimescale = 5f; - // m_bankingEfficiency = -0.3f; - // m_bankingMix = 0.8f; - // m_bankingTimescale = 1; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | - VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_AIRPLANE: - m_linearFrictionTimescale = new Vector3(200, 10, 5); - m_angularFrictionTimescale = new Vector3(20, 20, 20); -// m_lLinMotorVel = Vector3.Zero; - m_linearMotorTimescale = 2; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4; - m_VhoverHeight = 0; -// m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - // m_linearDeflectionEfficiency = 0.5f; - // m_linearDeflectionTimescale = 3; - // m_angularDeflectionEfficiency = 1; - // m_angularDeflectionTimescale = 2; - m_verticalAttractionEfficiency = 0.9f; - m_verticalAttractionTimescale = 2f; - // m_bankingEfficiency = 1; - // m_bankingMix = 0.7f; - // m_bankingTimescale = 2; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); - break; - case Vehicle.TYPE_BALLOON: - m_linearFrictionTimescale = new Vector3(5, 5, 5); - m_angularFrictionTimescale = new Vector3(10, 10, 10); - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - m_angularMotorTimescale = 6; - m_angularMotorDecayTimescale = 10; - m_VhoverHeight = 5; -// m_VhoverEfficiency = 0.8f; - m_VhoverTimescale = 10; - m_VehicleBuoyancy = 1; - // m_linearDeflectionEfficiency = 0; - // m_linearDeflectionTimescale = 5; - // m_angularDeflectionEfficiency = 0; - // m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 1f; - m_verticalAttractionTimescale = 100f; - // m_bankingEfficiency = 0; - // m_bankingMix = 0.7f; - // m_bankingTimescale = 5; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - break; - - } - }//end SetDefaultsForType - - internal void Enable(IntPtr pBody, OdeScene pParentScene) - { - if (m_type == Vehicle.TYPE_NONE) - return; - - m_body = pBody; - } - - internal void Step(float pTimestep, OdeScene pParentScene) - { - if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) - return; - frcount++; // used to limit debug comment output - if (frcount > 24) - frcount = 0; - - MoveLinear(pTimestep, pParentScene); - MoveAngular(pTimestep); - }// end Step - - internal void Halt() - { // Kill all motions, when non-physical - m_linearMotorDirection = Vector3.Zero; - m_lLinMotorDVel = Vector3.Zero; - m_lLinObjectVel = Vector3.Zero; - m_wLinObjectVel = Vector3.Zero; - m_angularMotorDirection = Vector3.Zero; - m_lastAngularVelocity = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - } - - private void UpdateLinDecay() - { - if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; - if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; - if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; - } // else let the motor decay on its own - - private void MoveLinear(float pTimestep, OdeScene _pParentScene) - { - Vector3 acceleration = new Vector3(0f, 0f, 0f); - - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - Quaternion irotq = Quaternion.Inverse(rotq); - d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame - Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); - acceleration = vel_now - m_wLinObjectVel; - m_lLinObjectVel = vel_now * irotq; - - if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate - { - if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) - { - float decayfactor = m_linearMotorDecayTimescale/pTimestep; - Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); - m_lLinMotorDVel -= decayAmount; - } - else - { - float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); - Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * pTimestep; - m_lLinMotorDVel -= decel; - } - if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) - { - m_lLinMotorDVel = Vector3.Zero; - } - else - { - if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; - if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; - if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; - } - } - - if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - if (m_linearMotorTimescale < 300.0f) - { - Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; - float linfactor = m_linearMotorTimescale/pTimestep; - Vector3 attackAmount = (attack_error/linfactor) * 1.3f; - m_lLinObjectVel += attackAmount; - } - if (m_linearFrictionTimescale.X < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.X / pTimestep; - float fricX = m_lLinObjectVel.X / fricfactor; - m_lLinObjectVel.X -= fricX; - } - if (m_linearFrictionTimescale.Y < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.Y / pTimestep; - float fricY = m_lLinObjectVel.Y / fricfactor; - m_lLinObjectVel.Y -= fricY; - } - if (m_linearFrictionTimescale.Z < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.Z / pTimestep; -//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor); - float fricZ = m_lLinObjectVel.Z / fricfactor; - m_lLinObjectVel.Z -= fricZ; - } - } - m_wLinObjectVel = m_lLinObjectVel * rotq; - // Add Gravity and Buoyancy - Vector3 grav = Vector3.Zero; - if(m_VehicleBuoyancy < 1.0f) - { - // There is some gravity, make a gravity force vector - // that is applied after object velocity. - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; - grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force - } // else its 1.0, no gravity. - - // Check if hovering - if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) - { - // We should hover, get the target height - d.Vector3 pos = d.BodyGetPosition(Body); - if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) - { - m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) - { - m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) - { - m_VhoverTargetHeight = m_VhoverHeight; - } - - if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) - { - // If body is aready heigher, use its height as target height - if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; - } - -// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped -// m_VhoverTimescale = 0f; // time to acheive height -// pTimestep is time since last frame,in secs - float herr0 = pos.Z - m_VhoverTargetHeight; - // Replace Vertical speed with correction figure if significant - if(Math.Abs(herr0) > 0.01f ) - { - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - m_wLinObjectVel.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); - //KF: m_VhoverEfficiency is not yet implemented - } - else - { - m_wLinObjectVel.Z = 0f; - } - } - else - { // not hovering, Gravity rules - m_wLinObjectVel.Z = vel_now.Z; -//if(frcount == 0) Console.WriteLine(" Z {0} a.Z {1}", m_wLinObjectVel.Z, acceleration.Z); - } - // Apply velocity - d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z); - // apply gravity force - d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); -//if(frcount == 0) Console.WriteLine("Grav {0}", grav); - } // end MoveLinear() - - private void UpdateAngDecay() - { - if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; - if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; - if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; - } // else let the motor decay on its own - - private void MoveAngular(float pTimestep) - { - /* - private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - - private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL - private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL - private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL - - private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor - private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body - */ -//if(frcount == 0) Console.WriteLine("MoveAngular "); - - // Get what the body is doing, this includes 'external' influences - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - Quaternion irotq = Quaternion.Inverse(rotq); - d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); - Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); - angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation - -//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); -// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel; -// Vector3 initavel = angObjectVel; - // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. - float atk_decayfactor = 23.0f / (m_angularMotorTimescale * pTimestep); - m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; - // Decay Angular Motor 2. - if (m_angularMotorDecayTimescale < 300.0f) - { -//#### - if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) - { - float decayfactor = (m_angularMotorDecayTimescale)/pTimestep; - Vector3 decayAmount = (m_angularMotorDVel/decayfactor); - m_angularMotorDVel -= decayAmount; - } - else - { - Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * pTimestep / m_angularMotorDecayTimescale; - m_angularMotorDVel -= decel; - } - - if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) - { - m_angularMotorDVel = Vector3.Zero; - } - else - { - if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; - if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; - if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; - } - } // end decay angular motor -//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); - -//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); - // Vertical attractor section - Vector3 vertattr = Vector3.Zero; - - if(m_verticalAttractionTimescale < 300) - { - float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep); - // get present body rotation -// d.Quaternion rot = d.BodyGetQuaternion(Body); -// Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - // make a vector pointing up - Vector3 verterr = Vector3.Zero; - verterr.Z = 1.0f; - // rotate it to Body Angle - verterr = verterr * rotq; - // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. - // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go - // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. - - if (verterr.Z < 0.0f) - { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to - // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. -//Console.WriteLine("InvertFlip"); - verterr.X = 2.0f - verterr.X; - verterr.Y = 2.0f - verterr.Y; - } - verterr *= 0.5f; - // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) - - if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) - { -//if(frcount == 0) - // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so - // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. - vertattr.X = verterr.Y; - vertattr.Y = - verterr.X; - vertattr.Z = 0f; -//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); - - // scaling appears better usingsquare-law - float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; - float bounce = 1.0f - damped; - // 0 = crit damp, 1 = bouncy - float oavz = angObjectVel.Z; // retain z velocity - angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy - angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); // damped, good @ < 90. - angObjectVel.Z = oavz; -//if(frcount == 0) Console.WriteLine("VA+"); -//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); - } - else - { - // else error is very small - angObjectVel.X = 0f; - angObjectVel.Y = 0f; -//if(frcount == 0) Console.WriteLine("VA0"); - } - } // else vertical attractor is off -//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); - - if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { // if motor or object have motion - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - - if (m_angularMotorTimescale < 300.0f) - { - Vector3 attack_error = m_angularMotorDVel - angObjectVel; - float angfactor = m_angularMotorTimescale/pTimestep; - Vector3 attackAmount = (attack_error/angfactor); - angObjectVel += attackAmount; -//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); -//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); - } - - angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / pTimestep); - angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / pTimestep); - angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / pTimestep); - } // else no signif. motion - -//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); - // Bank section tba - // Deflection section tba -//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); - - m_lastAngularVelocity = angObjectVel; -/* - if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f)) - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - } - else - { - m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. - } - */ -//if(frcount == 0) Console.WriteLine("angularLock {0}", m_angularLock); - - if (!m_angularLock.ApproxEquals(Vector3.One, 0.003f)) - { - if (m_angularLock.X == 0) - m_lastAngularVelocity.X = 0f; - if (m_angularLock.Y == 0) - m_lastAngularVelocity.Y = 0f; - if (m_angularLock.Z == 0) - m_lastAngularVelocity.Z = 0f; - } - // Apply to the body -// Vector3 aInc = m_lastAngularVelocity - initavel; -//if(frcount == 0) Console.WriteLine("Inc {0}", aInc); - m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation - - d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); -//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); - - } //end MoveAngular - } -} diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index fb6cb55..5d24388 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -22,32 +22,10 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. + * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs + * rolled into ODEPrim.cs */ -/* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - */ using System; using System.Collections.Generic; using System.Reflection; @@ -59,6 +37,7 @@ using Ode.NET; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; + namespace OpenSim.Region.Physics.OdePlugin { /// @@ -102,8 +81,8 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_APIDDamping = 0.5f; private bool m_useAPID = false; - // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), - // and are for non-VEHICLES only. + // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // do not confuse with VEHICLE HOVER private float m_PIDHoverHeight; private float m_PIDHoverTau; @@ -112,7 +91,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_targetHoverHeight; private float m_groundHeight; private float m_waterHeight; - private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + private float m_buoyancy; //m_buoyancy set by llSetBuoyancy() // private float m_tensor = 5f; private int body_autodisable_frames = 20; @@ -183,7 +162,7 @@ namespace OpenSim.Region.Physics.OdePlugin public bool outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; - public bool _zeroFlag; + public bool _zeroFlag; // if body has been stopped private bool m_lastUpdateSent; public IntPtr Body = IntPtr.Zero; @@ -198,18 +177,81 @@ namespace OpenSim.Region.Physics.OdePlugin public volatile bool childPrim; - private ODEDynamics m_vehicle; - internal int m_material = (int)Material.Wood; private int frcount = 0; // Used to limit dynamics debug output to + private IntPtr m_body = IntPtr.Zero; + + // Vehicle properties ============================================================================================ + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity + //requested by LSL + private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL + private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL + private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL + + private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor + private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity + private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL + private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL + + private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor +// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + + private Vector3 m_angularLock = Vector3.One; + + //Deflection properties + // private float m_angularDeflectionEfficiency = 0; + // private float m_angularDeflectionTimescale = 0; + // private float m_linearDeflectionEfficiency = 0; + // private float m_linearDeflectionTimescale = 0; + + //Banking properties + // private float m_bankingEfficiency = 0; + // private float m_bankingMix = 0; + // private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; +// private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 0f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped + private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. + + + + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) { - m_vehicle = new ODEDynamics(); - //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); ode = dode; if (!pos.IsFinite()) { @@ -302,7 +344,7 @@ namespace OpenSim.Region.Physics.OdePlugin { set { - +Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); // This only makes the object not collidable if the object // is physical or the object is modified somehow *IN THE FUTURE* // without this, if an avatar selects prim, they can walk right @@ -322,326 +364,563 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public void SetGeom(IntPtr geom) + public override bool IsPhysical { - prev_geom = prim_geom; - prim_geom = geom; -//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); - if (prim_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + get { return m_isphysical; } + set + { + m_isphysical = value; + if (!m_isphysical) + { // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + if (m_type != Vehicle.TYPE_NONE) Halt(); + } } + } - if (childPrim) + public void setPrimForRemoval() + { + m_taintremove = true; + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return iscolliding; } + set { iscolliding = value; } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get { return _position; } + + set { _position = value; + //m_log.Info("[PHYSICS]: " + _position.ToString()); + } + } + + public override Vector3 Size + { + get { return _size; } + set { - if (_parent != null && _parent is OdePrim) + if (value.IsFinite()) { - OdePrim parent = (OdePrim)_parent; -//Console.WriteLine("SetGeom calls ChildSetGeom"); - parent.ChildSetGeom(this); + _size = value; + } + else + { + m_log.Warn("[PHYSICS]: Got NaN Size on object"); } } - //m_log.Warn("Setting Geom to: " + prim_geom); } - + public override float Mass + { + get { return CalculateMass(); } + } - public void enableBodySoft() + public override Vector3 Force { - if (!childPrim) + //get { return Vector3.Zero; } + get { return m_force; } + set { - if (m_isphysical && Body != IntPtr.Zero) + if (value.IsFinite()) { - d.BodyEnable(Body); - if (m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Enable(Body, _parent_scene); + m_force = value; + } + else + { + m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object"); } - - m_disabled = false; } } - public void disableBodySoft() + public override int VehicleType { - m_disabled = true; + get { return (int)m_type; } + set { ProcessTypeChange((Vehicle)value); } + } - if (m_isphysical && Body != IntPtr.Zero) - { - d.BodyDisable(Body); - } + public override void VehicleFloatParam(int param, float value) + { + ProcessFloatVehicleParam((Vehicle) param, value); } - public void enableBody() + public override void VehicleVectorParam(int param, Vector3 value) { - // Don't enable this body if we're a child prim - // this should be taken care of in the parent function not here - if (!childPrim) + ProcessVectorVehicleParam((Vehicle) param, value); + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + ProcessRotationVehicleParam((Vehicle) param, rotation); + } + + public override void VehicleFlagsSet(int flags) + { + ProcessFlagsVehicleSet(flags); + } + + public override void VehicleFlagsRemove(int flags) + { + ProcessFlagsVehicleRemove(flags); + } + + public override void SetVolumeDetect(int param) + { + lock (_parent_scene.OdeLock) { - // Sets the geom to a body - Body = d.BodyCreate(_parent_scene.world); + m_isVolumeDetect = (param!=0); + } + } - setMass(); - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.BodySetQuaternion(Body, ref myrot); - d.GeomSetBody(prim_geom, Body); - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + public override Vector3 CenterOfMass + { + get { return Vector3.Zero; } + } - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + public override Vector3 GeometricCenter + { + get { return Vector3.Zero; } + } - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode (Body, false); + public override PrimitiveBaseShape Shape + { + set + { + _pbs = value; + m_taintshape = true; + } + } - m_interpenetrationcount = 0; - m_collisionscore = 0; - m_disabled = false; + public override Vector3 Velocity + { + get + { + // Averate previous velocity with the new one so + // client object interpolation works a 'little' better + if (_zeroFlag) + return Vector3.Zero; - // The body doesn't already have a finite rotation mode set here - /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) + Vector3 returnVelocity = Vector3.Zero; + returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; + return returnVelocity; + } + set + { + if (value.IsFinite()) { - createAMotor(m_angularlock); - } */ - if (m_vehicle.Type != Vehicle.TYPE_NONE) + _velocity = value; + + m_taintVelocity = value; + _parent_scene.AddPhysicsActorTaint(this); + } + else { - m_vehicle.Enable(Body, _parent_scene); + m_log.Warn("[PHYSICS]: Got NaN Velocity in Object"); } - _parent_scene.addActivePrim(this); } } - #region Mass Calculation - - private float CalculateMass() + public override Vector3 Torque { - float volume = 0; - - // No material is passed to the physics engines yet.. soo.. - // we're using the m_density constant in the class definition + get + { + if (!m_isphysical || Body == IntPtr.Zero) + return Vector3.Zero; - float returnMass = 0; + return _torque; + } - switch (_pbs.ProfileShape) + set { - case ProfileShape.Square: - // Profile Volume - - volume = _size.X*_size.Y*_size.Z; + if (value.IsFinite()) + { + m_taintTorque = value; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got NaN Torque in Object"); + } + } + } - // If the user has 'hollowed out' - // ProfileHollow is one of those 0 to 50000 values :P - // we like percentages better.. so turning into a percentage + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } - if (((float) _pbs.ProfileHollow/50000f) > 0.0) - { - float hollowAmount = (float) _pbs.ProfileHollow/50000f; + public override bool Kinematic + { + get { return false; } + set { } + } - // calculate the hollow volume by it's shape compared to the prim shape - float hollowVolume = 0; - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - // Cube Hollow volume calculation - float hollowsizex = _size.X*hollowAmount; - float hollowsizey = _size.Y*hollowAmount; - float hollowsizez = _size.Z*hollowAmount; - hollowVolume = hollowsizex*hollowsizey*hollowsizez; - break; + public override Quaternion Orientation + { + get { return _orientation; } + set + { + if (QuaternionIsFinite(value)) + { + _orientation = value; + } + else + m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - float hRadius = _size.X/2; - float hLength = _size.Z; + } + } + + + public override bool FloatOnWater + { + set { + m_taintCollidesWater = value; + _parent_scene.AddPhysicsActorTaint(this); + } + } - // pi * r2 * h - hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount); - break; + public override void SetMomentum(Vector3 momentum) + { + } - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); + } + } + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + // For RotLookAt + public override Quaternion APIDTarget { set { m_APIDTarget = value; } } + public override bool APIDActive { set { m_useAPID = value; } } + public override float APIDStrength { set { m_APIDStrength = value; } } + public override float APIDDamping { set { m_APIDDamping = value; } } - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount); - break; + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } - default: - hollowVolume = 0; - break; - } - volume = volume - hollowVolume; - } + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } - break; - case ProfileShape.Circle: - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - // Cylinder - float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z); - float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z); + public override Vector3 Acceleration + { + get { return _acceleration; } + } - // Approximating the cylinder's irregularity. - if (volume1 > volume2) - { - volume = (float)volume1 - (volume1 - volume2); - } - else if (volume2 > volume1) - { - volume = (float)volume2 - (volume2 - volume1); - } - else - { - // Regular cylinder - volume = volume1; - } - } - else - { - // We don't know what the shape is yet, so use default - volume = _size.X * _size.Y * _size.Z; - } - // If the user has 'hollowed out' - // ProfileHollow is one of those 0 to 50000 values :P - // we like percentages better.. so turning into a percentage - if (((float)_pbs.ProfileHollow / 50000f) > 0.0) - { - float hollowAmount = (float)_pbs.ProfileHollow / 50000f; + public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything. + { + _acceleration = accel; + } - // calculate the hollow volume by it's shape compared to the prim shape - float hollowVolume = 0; - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - float hRadius = _size.X / 2; - float hLength = _size.Z; + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + lock (m_forcelist) + m_forcelist.Add(force); - // pi * r2 * h - hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount); - break; + m_taintforce = true; + } + else + { + m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object"); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } - case HollowShape.Square: - // Cube Hollow volume calculation - float hollowsizex = _size.X * hollowAmount; - float hollowsizey = _size.Y * hollowAmount; - float hollowsizez = _size.Z * hollowAmount; - hollowVolume = hollowsizex * hollowsizey * hollowsizez; - break; + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + m_angularforcelist.Add(force); + m_taintaddangularforce = true; + } + else + { + m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object"); + } + } - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + public override Vector3 RotationalVelocity + { + get + { +/* Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); - break; + if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) + return pv; +*/ + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + } + else + { + m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object"); + } + } + } - default: - hollowVolume = 0; - break; - } - volume = volume - hollowVolume; - } - break; + public override void CrossingFailure() + { + m_crossingfailures++; + if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + base.RaiseOutOfBounds(_position); + return; + } + else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); + } + } - case ProfileShape.HalfCircle: - if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - if (_size.X == _size.Y && _size.Y == _size.Z) - { - // regular sphere - // v = 4/3 * pi * r^3 - float sradius3 = (float)Math.Pow((_size.X / 2), 3); - volume = (float)((4f / 3f) * Math.PI * sradius3); - } - else - { - // we treat this as a box currently - volume = _size.X * _size.Y * _size.Z; - } - } - else - { - // We don't know what the shape is yet, so use default - volume = _size.X * _size.Y * _size.Z; - } - break; + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } - case ProfileShape.EquilateralTriangle: - /* - v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h + public override void link(PhysicsActor obj) + { + m_taintparent = obj; + } - // seed mesh - Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f); - Vertex PM = new Vertex(+0.5f, 0f, 0.0f); - Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f); - */ - float xA = -0.25f * _size.X; - float yA = -0.45f * _size.Y; + public override void delink() + { + m_taintparent = null; + } - float xB = 0.5f * _size.X; - float yB = 0; + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + m_taintAngularLock = axis; + } + else + { + m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object"); + } + } - float xC = -0.25f * _size.X; - float yC = 0.45f * _size.Y; - volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z); + public void SetGeom(IntPtr geom) + { + prev_geom = prim_geom; + prim_geom = geom; +//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + if (childPrim) + { + if (_parent != null && _parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; +//Console.WriteLine("SetGeom calls ChildSetGeom"); + parent.ChildSetGeom(this); + } + } + //m_log.Warn("Setting Geom to: " + prim_geom); + } + + public void enableBodySoft() + { + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero) + { + d.BodyEnable(Body); + if (m_type != Vehicle.TYPE_NONE) + Enable(Body, _parent_scene); + } + + m_disabled = false; + } + } + + public void disableBodySoft() + { + m_disabled = true; + + if (m_isphysical && Body != IntPtr.Zero) + { + d.BodyDisable(Body); + } + } + + public void enableBody() + { + // Don't enable this body if we're a child prim + // this should be taken care of in the parent function not here + if (!childPrim) + { + // Sets the geom to a body + Body = d.BodyCreate(_parent_scene.world); + + setMass(); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.BodySetQuaternion(Body, ref myrot); + d.GeomSetBody(prim_geom, Body); + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode (Body, false); + + m_interpenetrationcount = 0; + m_collisionscore = 0; + m_disabled = false; + + if (m_type != Vehicle.TYPE_NONE) + { + Enable(Body, _parent_scene); + } + + _parent_scene.addActivePrim(this); + } + } + + #region Mass Calculation + + private float CalculateMass() + { + float volume = 0; + + // No material is passed to the physics engines yet.. soo.. + // we're using the m_density constant in the class definition + + float returnMass = 0; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // Profile Volume + + volume = _size.X*_size.Y*_size.Z; // If the user has 'hollowed out' // ProfileHollow is one of those 0 to 50000 values :P // we like percentages better.. so turning into a percentage - float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f); - if (((float)fhollowFactor / 50000f) > 0.0) + + if (((float) _pbs.ProfileHollow/50000f) > 0.0) { - float hollowAmount = (float)fhollowFactor / 50000f; + float hollowAmount = (float) _pbs.ProfileHollow/50000f; // calculate the hollow volume by it's shape compared to the prim shape float hollowVolume = 0; switch (_pbs.HollowShape) { - case HollowShape.Same: - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y - - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); - break; - case HollowShape.Square: + case HollowShape.Same: // Cube Hollow volume calculation - float hollowsizex = _size.X * hollowAmount; - float hollowsizey = _size.Y * hollowAmount; - float hollowsizez = _size.Z * hollowAmount; - hollowVolume = hollowsizex * hollowsizey * hollowsizez; + float hollowsizex = _size.X*hollowAmount; + float hollowsizey = _size.Y*hollowAmount; + float hollowsizez = _size.Z*hollowAmount; + hollowVolume = hollowsizex*hollowsizey*hollowsizez; break; case HollowShape.Circle: // Hollow shape is a perfect cyllinder in respect to the cube's scale // Cyllinder hollow volume calculation - float hRadius = _size.X / 2; + float hRadius = _size.X/2; float hLength = _size.Z; // pi * r2 * h - hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount); + hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount); + break; + + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount); break; default: @@ -650,28 +929,194 @@ namespace OpenSim.Region.Physics.OdePlugin } volume = volume - hollowVolume; } - break; - default: - // we don't have all of the volume formulas yet so - // use the common volume formula for all - volume = _size.X*_size.Y*_size.Z; break; - } + case ProfileShape.Circle: + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + // Cylinder + float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z); + float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z); - // Calculate Path cut effect on volume - // Not exact, in the triangle hollow example - // They should never be zero or less then zero.. - // we'll ignore it if it's less then zero + // Approximating the cylinder's irregularity. + if (volume1 > volume2) + { + volume = (float)volume1 - (volume1 - volume2); + } + else if (volume2 > volume1) + { + volume = (float)volume2 - (volume2 - volume1); + } + else + { + // Regular cylinder + volume = volume1; + } + } + else + { + // We don't know what the shape is yet, so use default + volume = _size.X * _size.Y * _size.Z; + } + // If the user has 'hollowed out' + // ProfileHollow is one of those 0 to 50000 values :P + // we like percentages better.. so turning into a percentage - // ProfileEnd and ProfileBegin are values - // from 0 to 50000 + if (((float)_pbs.ProfileHollow / 50000f) > 0.0) + { + float hollowAmount = (float)_pbs.ProfileHollow / 50000f; - // Turning them back into percentages so that I can cut that percentage off the volume + // calculate the hollow volume by it's shape compared to the prim shape + float hollowVolume = 0; + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + float hRadius = _size.X / 2; + float hLength = _size.Z; - float PathCutEndAmount = _pbs.ProfileEnd; - float PathCutStartAmount = _pbs.ProfileBegin; - if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f) + // pi * r2 * h + hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount); + break; + + case HollowShape.Square: + // Cube Hollow volume calculation + float hollowsizex = _size.X * hollowAmount; + float hollowsizey = _size.Y * hollowAmount; + float hollowsizez = _size.Z * hollowAmount; + hollowVolume = hollowsizex * hollowsizey * hollowsizez; + break; + + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); + break; + + default: + hollowVolume = 0; + break; + } + volume = volume - hollowVolume; + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + if (_size.X == _size.Y && _size.Y == _size.Z) + { + // regular sphere + // v = 4/3 * pi * r^3 + float sradius3 = (float)Math.Pow((_size.X / 2), 3); + volume = (float)((4f / 3f) * Math.PI * sradius3); + } + else + { + // we treat this as a box currently + volume = _size.X * _size.Y * _size.Z; + } + } + else + { + // We don't know what the shape is yet, so use default + volume = _size.X * _size.Y * _size.Z; + } + break; + + case ProfileShape.EquilateralTriangle: + /* + v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h + + // seed mesh + Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f); + Vertex PM = new Vertex(+0.5f, 0f, 0.0f); + Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f); + */ + float xA = -0.25f * _size.X; + float yA = -0.45f * _size.Y; + + float xB = 0.5f * _size.X; + float yB = 0; + + float xC = -0.25f * _size.X; + float yC = 0.45f * _size.Y; + + volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z); + + // If the user has 'hollowed out' + // ProfileHollow is one of those 0 to 50000 values :P + // we like percentages better.. so turning into a percentage + float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f); + if (((float)fhollowFactor / 50000f) > 0.0) + { + float hollowAmount = (float)fhollowFactor / 50000f; + + // calculate the hollow volume by it's shape compared to the prim shape + float hollowVolume = 0; + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); + break; + + case HollowShape.Square: + // Cube Hollow volume calculation + float hollowsizex = _size.X * hollowAmount; + float hollowsizey = _size.Y * hollowAmount; + float hollowsizez = _size.Z * hollowAmount; + hollowVolume = hollowsizex * hollowsizey * hollowsizez; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + float hRadius = _size.X / 2; + float hLength = _size.Z; + + // pi * r2 * h + hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount); + break; + + default: + hollowVolume = 0; + break; + } + volume = volume - hollowVolume; + } + break; + + default: + // we don't have all of the volume formulas yet so + // use the common volume formula for all + volume = _size.X*_size.Y*_size.Z; + break; + } + + // Calculate Path cut effect on volume + // Not exact, in the triangle hollow example + // They should never be zero or less then zero.. + // we'll ignore it if it's less then zero + + // ProfileEnd and ProfileBegin are values + // from 0 to 50000 + + // Turning them back into percentages so that I can cut that percentage off the volume + + float PathCutEndAmount = _pbs.ProfileEnd; + float PathCutStartAmount = _pbs.ProfileBegin; + if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f) { float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f); @@ -894,9 +1339,8 @@ namespace OpenSim.Region.Physics.OdePlugin // } } - public void ProcessTaints(float timestep) + public void ProcessTaints(float timestep) //============================================================================= { -//Console.WriteLine("ProcessTaints for " + m_primName ); if (m_taintadd) { changeadd(timestep); @@ -985,30 +1429,6 @@ namespace OpenSim.Region.Physics.OdePlugin { //Console.WriteLine("Alock changed to {0}", m_taintAngularLock); m_angularlock = m_taintAngularLock; - m_vehicle.SetAngularLock(m_angularlock); - - -/* - if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) - { - //d.BodySetFiniteRotationMode(Body, 0); - //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); - createAMotor(m_taintAngularLock); - } - else - { - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - } - } - } - // Store this for later in case we get turned into a separate body - m_angularlock = m_taintAngularLock; - m_vehicle.SetAngularLock(m_angularlock); - } */ } } @@ -1151,11 +1571,6 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_collisionscore = 0; prm.m_disabled = false; - // The body doesn't already have a finite rotation mode set here - /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) - { - prm.createAMotor(m_angularlock); - } */ prm.Body = Body; _parent_scene.addActivePrim(prm); } @@ -1194,13 +1609,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionscore = 0; m_disabled = false; - // The body doesn't already have a finite rotation mode set here - /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) - { - createAMotor(m_angularlock); - } */ d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); - if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene); + if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene); _parent_scene.addActivePrim(this); } } @@ -1267,17 +1677,12 @@ namespace OpenSim.Region.Physics.OdePlugin //Console.WriteLine("childrenPrim.Remove " + odePrim); childrenPrim.Remove(odePrim); } - - - if (Body != IntPtr.Zero) { _parent_scene.remActivePrim(this); } - - lock (childrenPrim) { foreach (OdePrim prm in childrenPrim) @@ -1286,8 +1691,6 @@ namespace OpenSim.Region.Physics.OdePlugin ParentPrim(prm); } } - - } private void changeSelectedStatus(float timestep) @@ -1515,17 +1918,22 @@ namespace OpenSim.Region.Physics.OdePlugin public void changemove(float timestep) { +//Console.WriteLine("changemove for {0}", m_primName ); + if (m_isphysical) { - - if (!m_disabled && !m_taintremove && !childPrim) +//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim); +// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! + if (!m_taintremove && !childPrim) { +//Console.WriteLine("physOK"); if (Body == IntPtr.Zero) enableBody(); //Prim auto disable after 20 frames, //if you move it, re-enable the prim manually. if (_parent != null) { +//Console.WriteLine("physChild"); if (m_linkJoint != IntPtr.Zero) { d.JointDestroy(m_linkJoint); @@ -1534,6 +1942,7 @@ namespace OpenSim.Region.Physics.OdePlugin } if (Body != IntPtr.Zero) { +//Console.WriteLine("physNotIPZ"); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); if (_parent != null) @@ -1549,9 +1958,9 @@ Console.WriteLine(" JointCreateFixed"); } } d.BodyEnable(Body); - if (m_vehicle.Type != Vehicle.TYPE_NONE) + if (m_type != Vehicle.TYPE_NONE) { - m_vehicle.Enable(Body, _parent_scene); + Enable(Body, _parent_scene); } } else @@ -1566,6 +1975,7 @@ Console.WriteLine(" JointCreateFixed"); } else { +//Console.WriteLine("NONphys"); // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); _parent_scene.waitForSpaceUnlock(m_targetSpace); @@ -1589,433 +1999,234 @@ Console.WriteLine(" JointCreateFixed"); m_taintposition = _position; } - public void Move(float timestep) - { - float fx = 0; - float fy = 0; - float fz = 0; - - frcount++; // used to limit debug comment output - if (frcount > 100) - frcount = 0; - - if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. - { -//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type + - // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); - if (m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(timestep, _parent_scene); - } - else - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - // NON-'VEHICLES' are dealt with here - // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked. - -// NB this may be wrong - may lock global axis! Should be LOCAL axis! - if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f)) - { - d.Vector3 avel2 = d.BodyGetAngularVel(Body); - if (m_angularlock.X == 0) - avel2.X = 0; - if (m_angularlock.Y == 0) - avel2.Y = 0; - if (m_angularlock.Z == 0) - avel2.Z = 0; - d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); - } - //float PID_P = 900.0f; - float m_mass = CalculateMass(); -// fz = 0f; - //m_log.Info(m_collisionFlags.ToString()); + public void rotate(float timestep) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + if (Body != IntPtr.Zero) + { + // KF: If this is a root prim do BodySet + d.BodySetQuaternion(Body, ref myrot); + } + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + resetCollisionAccounting(); + m_taintrot = _orientation; + } - - //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. - // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up - // NB Prims in ODE are no subject to global gravity - fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass + private void resetCollisionAccounting() + { + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_disabled = false; + } - if (m_usePID) - { -//if(frcount == 0) Console.WriteLine("PID " + m_primName); - // KF - this is for object MoveToTarget. - - //if (!d.BodyIsEnabled(Body)) - //d.BodySetForce(Body, 0f, 0f, 0f); + public void changedisable(float timestep) + { + m_disabled = true; + if (Body != IntPtr.Zero) + { + d.BodyDisable(Body); + Body = IntPtr.Zero; + } - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + m_taintdisable = false; + } - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - //PidStatus = true; + public void changePhysicsStatus(float timestep) + { + if (m_isphysical == true) + { + if (Body == IntPtr.Zero) + { + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeshape(2f); + } + else + { + enableBody(); + } + } + } + else + { + if (Body != IntPtr.Zero) + { + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + - // PhysicsVector vec = new PhysicsVector(); - d.Vector3 vel = d.BodyGetLinearVel(Body); + if (prim_geom != IntPtr.Zero) + { + try + { + d.GeomDestroy(prim_geom); + prim_geom = IntPtr.Zero; + _mesh = null; + } + catch (System.AccessViolationException) + { + prim_geom = IntPtr.Zero; + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + } +//Console.WriteLine("changePhysicsStatus for " + m_primName ); + changeadd(2f); + } + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); + } + } + else + { + disableBody(); + } + } + } - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); + changeSelectedStatus(timestep); - // if velocity is zero, use position control; otherwise, velocity control + resetCollisionAccounting(); + m_taintPhysics = m_isphysical; + } - if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions + public void changesize(float timestamp) + { + + string oldname = _parent_scene.geom_name_map[prim_geom]; - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + if (_size.X <= 0) _size.X = 0.01f; + if (_size.Y <= 0) _size.Y = 0.01f; + if (_size.Z <= 0) _size.Z = 0.01f; - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + // Cleanup of old prim geometry + if (_mesh != null) + { + // Cleanup meshing here + } + //kill body to rebuild + if (IsPhysical && Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); + } + } + else + { + disableBody(); + } + } + if (d.SpaceQuery(m_targetSpace, prim_geom)) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + d.SpaceRemove(m_targetSpace, prim_geom); + } + d.GeomDestroy(prim_geom); + prim_geom = IntPtr.Zero; + // we don't need to do space calculation because the client sends a position update also. - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end if (m_usePID) + // Construction of new prim + if (_parent_scene.needsMeshing(_pbs)) + { + float meshlod = _parent_scene.meshSculptLOD; - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - if (m_useHoverPID && !m_usePID) - { -//Console.WriteLine("Hover " + m_primName); - - // If we're using the PID controller, then we have no gravity - fz = (-1 * _parent_scene.gravityz) * m_mass; + if (IsPhysical) + meshlod = _parent_scene.MeshSculptphysicalLOD; + // Don't need to re-enable body.. it's done in SetMesh - // no lock; for now it's only called from within Simulate() + IMesh mesh = null; - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + if (_parent_scene.needsMeshing(_pbs)) + mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } + //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); +//Console.WriteLine("changesize 1"); + CreateGeom(m_targetSpace, mesh); - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - + + } + else + { + _mesh = null; +//Console.WriteLine("changesize 2"); + CreateGeom(m_targetSpace, _mesh); + } - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); + if (IsPhysical && Body == IntPtr.Zero && !childPrim) + { + // Re creates body on size. + // EnableBody also does setMass() + enableBody(); + d.BodyEnable(Body); + } - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; + _parent_scene.geom_name_map[prim_geom] = oldname; - } // end switch (m_PIDHoverType) + changeSelectedStatus(timestamp); + if (childPrim) + { + if (_parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildSetGeom(this); + } + } + resetCollisionAccounting(); + m_taintsize = _size; + } + - _target_velocity = - new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); + public void changefloatonwater(float timestep) + { + m_collidesWater = m_taintCollidesWater; - // if velocity is zero, use position control; otherwise, velocity control + if (prim_geom != IntPtr.Zero) + { + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } + else + { + m_collisionFlags &= ~CollisionCategories.Water; + } + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions + public void changeshape(float timestamp) + { + string oldname = _parent_scene.geom_name_map[prim_geom]; - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - d.BodyAddForce(Body, 0, 0, fz); - //KF this prevents furthur motions return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end m_useHoverPID && !m_usePID - - if (m_useAPID) - { - // RotLookAt, apparently overrides all other rotation sources. Inputs: - // Quaternion m_APIDTarget - // float m_APIDStrength // From SL experiments, this is the time to get there - // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly - // Also in SL the mass of the object has no effect on time to get there. - // Factors: -//if(frcount == 0) Console.WriteLine("APID "); - // get present body rotation - float limit = 1.0f; - float scaler = 50f; // adjusts damping time - float RLAservo = 0f; - - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; - float diff_angle; - Vector3 diff_axis; - rot_diff.GetAxisAngle(out diff_axis, out diff_angle); - diff_axis.Normalize(); - if(diff_angle > 0.01f) // diff_angle is always +ve - { -// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); - Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); - rotforce = rotforce * rotq; - if(diff_angle > limit) diff_angle = limit; // cap the rotate rate -// RLAservo = timestep / m_APIDStrength * m_mass * scaler; - // rotforce = rotforce * RLAservo * diff_angle ; - // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); - RLAservo = timestep / m_APIDStrength * scaler; - rotforce = rotforce * RLAservo * diff_angle ; - - if (m_angularlock.X == 0) - rotforce.X = 0; - if (m_angularlock.Y == 0) - rotforce.Y = 0; - if (m_angularlock.Z == 0) - rotforce.Z = 0; - - - - - d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); -//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); - } -//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); - } // end m_useAPID - - fx *= m_mass; - fy *= m_mass; - //fz *= m_mass; - - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - //m_taintdisable = true; - //base.RaiseOutOfBounds(Position); - //d.BodySetLinearVel(Body, fx, fy, 0f); - if (!d.BodyIsEnabled(Body)) - { - // A physical body at rest on a surface will auto-disable after a while, - // this appears to re-enable it incase the surface it is upon vanishes, - // and the body should fall again. - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0, 0, 0); - enableBodySoft(); - } - - // 35x10 = 350n times the mass per second applied maximum. - float nmax = 35f * m_mass; - float nmin = -35f * m_mass; - - - if (fx > nmax) - fx = nmax; - if (fx < nmin) - fx = nmin; - if (fy > nmax) - fy = nmax; - if (fy < nmin) - fy = nmin; - d.BodyAddForce(Body, fx, fy, fz); -//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - } - } - else - { // is not physical, or is not a body or is selected - // _zeroPosition = d.BodyGetPosition(Body); - return; -//Console.WriteLine("Nothing " + m_primName); - - } - } - - - - public void rotate(float timestep) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - if (Body != IntPtr.Zero) - { - // KF: If this is a root prim do BodySet - d.BodySetQuaternion(Body, ref myrot); - /* ### if (m_isphysical) - { - if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } */ - } - else - { - // daughter prim, do Geom set - d.GeomSetQuaternion(prim_geom, ref myrot); - } - - resetCollisionAccounting(); - m_taintrot = _orientation; - } - - private void resetCollisionAccounting() - { - m_collisionscore = 0; - m_interpenetrationcount = 0; - m_disabled = false; - } - - public void changedisable(float timestep) - { - m_disabled = true; - if (Body != IntPtr.Zero) - { - d.BodyDisable(Body); - Body = IntPtr.Zero; - } - - m_taintdisable = false; - } - - public void changePhysicsStatus(float timestep) - { - if (m_isphysical == true) - { - if (Body == IntPtr.Zero) - { - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeshape(2f); - } - else - { - enableBody(); - } - } - } - else - { - if (Body != IntPtr.Zero) - { - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - - - if (prim_geom != IntPtr.Zero) - { - try - { - d.GeomDestroy(prim_geom); - prim_geom = IntPtr.Zero; - _mesh = null; - } - catch (System.AccessViolationException) - { - prim_geom = IntPtr.Zero; - m_log.Error("[PHYSICS]: PrimGeom dead"); - } - } -//Console.WriteLine("changePhysicsStatus for " + m_primName ); - changeadd(2f); - } - if (childPrim) - { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this); - } - } - else - { - disableBody(); - } - } - } - - changeSelectedStatus(timestep); - - resetCollisionAccounting(); - m_taintPhysics = m_isphysical; - } - - public void changesize(float timestamp) - { - - string oldname = _parent_scene.geom_name_map[prim_geom]; - - if (_size.X <= 0) _size.X = 0.01f; - if (_size.Y <= 0) _size.Y = 0.01f; - if (_size.Z <= 0) _size.Z = 0.01f; - - // Cleanup of old prim geometry - if (_mesh != null) - { - // Cleanup meshing here - } - //kill body to rebuild + // Cleanup of old prim geometry and Bodies if (IsPhysical && Body != IntPtr.Zero) { if (childPrim) @@ -2031,133 +2242,25 @@ Console.WriteLine(" JointCreateFixed"); disableBody(); } } - if (d.SpaceQuery(m_targetSpace, prim_geom)) + try { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - d.SpaceRemove(m_targetSpace, prim_geom); + d.GeomDestroy(prim_geom); + } + catch (System.AccessViolationException) + { + prim_geom = IntPtr.Zero; + m_log.Error("[PHYSICS]: PrimGeom dead"); } - d.GeomDestroy(prim_geom); prim_geom = IntPtr.Zero; // we don't need to do space calculation because the client sends a position update also. - + if (_size.X <= 0) _size.X = 0.01f; + if (_size.Y <= 0) _size.Y = 0.01f; + if (_size.Z <= 0) _size.Z = 0.01f; // Construction of new prim + if (_parent_scene.needsMeshing(_pbs)) { - float meshlod = _parent_scene.meshSculptLOD; - - if (IsPhysical) - meshlod = _parent_scene.MeshSculptphysicalLOD; - // Don't need to re-enable body.. it's done in SetMesh - - IMesh mesh = null; - - if (_parent_scene.needsMeshing(_pbs)) - mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); - - //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); -//Console.WriteLine("changesize 1"); - CreateGeom(m_targetSpace, mesh); - - - } - else - { - _mesh = null; -//Console.WriteLine("changesize 2"); - CreateGeom(m_targetSpace, _mesh); - } - - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - - //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); - if (IsPhysical && Body == IntPtr.Zero && !childPrim) - { - // Re creates body on size. - // EnableBody also does setMass() - enableBody(); - d.BodyEnable(Body); - } - - _parent_scene.geom_name_map[prim_geom] = oldname; - - changeSelectedStatus(timestamp); - if (childPrim) - { - if (_parent is OdePrim) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildSetGeom(this); - } - } - resetCollisionAccounting(); - m_taintsize = _size; - } - - - - public void changefloatonwater(float timestep) - { - m_collidesWater = m_taintCollidesWater; - - if (prim_geom != IntPtr.Zero) - { - if (m_collidesWater) - { - m_collisionFlags |= CollisionCategories.Water; - } - else - { - m_collisionFlags &= ~CollisionCategories.Water; - } - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - } - - public void changeshape(float timestamp) - { - string oldname = _parent_scene.geom_name_map[prim_geom]; - - // Cleanup of old prim geometry and Bodies - if (IsPhysical && Body != IntPtr.Zero) - { - if (childPrim) - { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this); - } - } - else - { - disableBody(); - } - } - try - { - d.GeomDestroy(prim_geom); - } - catch (System.AccessViolationException) - { - prim_geom = IntPtr.Zero; - m_log.Error("[PHYSICS]: PrimGeom dead"); - } - prim_geom = IntPtr.Zero; - // we don't need to do space calculation because the client sends a position update also. - if (_size.X <= 0) _size.X = 0.01f; - if (_size.Y <= 0) _size.Y = 0.01f; - if (_size.Z <= 0) _size.Z = 0.01f; - // Construction of new prim - - if (_parent_scene.needsMeshing(_pbs)) - { - // Don't need to re-enable body.. it's done in SetMesh + // Don't need to re-enable body.. it's done in SetMesh float meshlod = _parent_scene.meshSculptLOD; if (IsPhysical) @@ -2319,392 +2422,834 @@ Console.WriteLine(" JointCreateFixed"); m_taintVelocity = Vector3.Zero; } - public override bool IsPhysical + public void UpdatePositionAndVelocity() { - get { return m_isphysical; } - set - { - m_isphysical = value; - if (!m_isphysical) - { // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Halt(); - } - } - } + return; // moved to the MOVE() method + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null) + { + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + if (Body != (IntPtr)0) // FIXME -> or if it is a joint + { + d.Vector3 vec = d.BodyGetPosition(Body); + d.Quaternion ori = d.BodyGetQuaternion(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + d.Vector3 torque = d.BodyGetTorque(Body); + _torque = new Vector3(torque.X, torque.Y, torque.Z); + Vector3 l_position = Vector3.Zero; + Quaternion l_orientation = Quaternion.Identity; - public void setPrimForRemoval() - { - m_taintremove = true; - } + // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) + //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - public override bool Flying - { - // no flying prims for you - get { return false; } - set { } - } + m_lastposition = _position; + m_lastorientation = _orientation; + + l_position.X = vec.X; + l_position.Y = vec.Y; + l_position.Z = vec.Z; + l_orientation.X = ori.X; + l_orientation.Y = ori.Y; + l_orientation.Z = ori.Z; + l_orientation.W = ori.W; + +// if(l_position.Y != m_lastposition.Y){ +// Console.WriteLine("UP&V {0} {1}", m_primName, l_position); +// } - public override bool IsColliding - { - get { return iscolliding; } - set { iscolliding = value; } - } + if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) + { + //base.RaiseOutOfBounds(l_position); - public override bool CollidingGround - { - get { return false; } - set { return; } - } + if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + _position = l_position; + //_parent_scene.remActivePrim(this); + if (_parent == null) + base.RequestPhysicsterseUpdate(); + return; + } + else + { + if (_parent == null) + base.RaiseOutOfBounds(l_position); + return; + } + } - public override bool CollidingObj - { - get { return false; } - set { return; } - } + if (l_position.Z < 0) + { + // This is so prim that get lost underground don't fall forever and suck up + // + // Sim resources and memory. + // Disables the prim's movement physics.... + // It's a hack and will generate a console message if it fails. - public override bool ThrottleUpdates - { - get { return m_throttleUpdates; } - set { m_throttleUpdates = value; } - } + //IsPhysical = false; + if (_parent == null) + base.RaiseOutOfBounds(_position); - public override bool Stopped - { - get { return _zeroFlag; } - } + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; - public override Vector3 Position - { - get { return _position; } + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; - set { _position = value; - //m_log.Info("[PHYSICS]: " + _position.ToString()); - } - } + if (_parent == null) + base.RequestPhysicsterseUpdate(); - public override Vector3 Size - { - get { return _size; } - set - { - if (value.IsFinite()) - { - _size = value; - } - else - { - m_log.Warn("[PHYSICS]: Got NaN Size on object"); - } - } - } + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + //outofBounds = true; + } - public override float Mass - { - get { return CalculateMass(); } - } + //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); +//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); + if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) +// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) + && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large + { + _zeroFlag = true; +//Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } - public override Vector3 Force - { - //get { return Vector3.Zero; } - get { return m_force; } - set - { - if (value.IsFinite()) - { - m_force = value; + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + //_orientation.w = 0f; + //_orientation.X = 0f; + //_orientation.Y = 0f; + //_orientation.Z = 0f; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } + } + + m_lastVelocity = _velocity; + + _position = l_position; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _acceleration = ((_velocity - m_lastVelocity) / 0.1f); + _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); + //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); + +// if (_velocity.ApproxEquals(pv, 0.5f)) ???? Disregard rotational vel if lin vel is < 0.5 ????? +// { +// m_rotationalVelocity = pv;/ + +// } +// else +// { + m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); +// } + + //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } + } + else + { + throttleCounter++; + } + } + m_lastposition = l_position; } else { - m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object"); + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; } } } - public override int VehicleType + public Matrix4 FromDMass(d.Mass pMass) { - get { return (int)m_vehicle.Type; } - set { m_vehicle.ProcessTypeChange((Vehicle)value); } + Matrix4 obj; + obj.M11 = pMass.I.M00; + obj.M12 = pMass.I.M01; + obj.M13 = pMass.I.M02; + obj.M14 = 0; + obj.M21 = pMass.I.M10; + obj.M22 = pMass.I.M11; + obj.M23 = pMass.I.M12; + obj.M24 = 0; + obj.M31 = pMass.I.M20; + obj.M32 = pMass.I.M21; + obj.M33 = pMass.I.M22; + obj.M34 = 0; + obj.M41 = 0; + obj.M42 = 0; + obj.M43 = 0; + obj.M44 = 1; + return obj; } - public override void VehicleFloatParam(int param, float value) + public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) { - m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value); + obj.I.M00 = pMat[0, 0]; + obj.I.M01 = pMat[0, 1]; + obj.I.M02 = pMat[0, 2]; + obj.I.M10 = pMat[1, 0]; + obj.I.M11 = pMat[1, 1]; + obj.I.M12 = pMat[1, 2]; + obj.I.M20 = pMat[2, 0]; + obj.I.M21 = pMat[2, 1]; + obj.I.M22 = pMat[2, 2]; + return obj; } - public override void VehicleVectorParam(int param, Vector3 value) + public override void SubscribeEvents(int ms) { - m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value); + m_eventsubscription = ms; + _parent_scene.addCollisionEventReporting(this); } - public override void VehicleRotationParam(int param, Quaternion rotation) - { - m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); - } - - public override void VehicleFlagsSet(int flags) - { - m_vehicle.ProcessFlagsVehicleSet(flags); - } - - public override void VehicleFlagsRemove(int flags) + public override void UnSubscribeEvents() { - m_vehicle.ProcessFlagsVehicleRemove(flags); + _parent_scene.remCollisionEventReporting(this); + m_eventsubscription = 0; } - - public override void SetVolumeDetect(int param) + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { - lock (_parent_scene.OdeLock) - { - m_isVolumeDetect = (param!=0); - } + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + CollisionEventsThisFrame.addCollider(CollidedWith, contact); } - public override Vector3 CenterOfMass + public void SendCollisions() { - get { return Vector3.Zero; } + if (CollisionEventsThisFrame == null) + return; + + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) + CollisionEventsThisFrame = null; + else + CollisionEventsThisFrame = new CollisionEventUpdate(); } - public override Vector3 GeometricCenter + public override bool SubscribedEvents() { - get { return Vector3.Zero; } + if (m_eventsubscription > 0) + return true; + return false; } - public override PrimitiveBaseShape Shape + public static Matrix4 Inverse(Matrix4 pMat) { - set + if (determinant3x3(pMat) == 0) { - _pbs = value; - m_taintshape = true; + return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible } + + + + return (Adjoint(pMat) / determinant3x3(pMat)); } - public override Vector3 Velocity + public static Matrix4 Adjoint(Matrix4 pMat) { - get - { - // Averate previous velocity with the new one so - // client object interpolation works a 'little' better - if (_zeroFlag) - return Vector3.Zero; - - Vector3 returnVelocity = Vector3.Zero; - returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; - returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; - returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; - return returnVelocity; - } - set + Matrix4 adjointMatrix = new Matrix4(); + for (int i=0; i<4; i++) { - if (value.IsFinite()) - { - _velocity = value; - - m_taintVelocity = value; - _parent_scene.AddPhysicsActorTaint(this); - } - else + for (int j=0; j<4; j++) { - m_log.Warn("[PHYSICS]: Got NaN Velocity in Object"); + Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); } - } + + adjointMatrix = Transpose(adjointMatrix); + return adjointMatrix; } - public override Vector3 Torque + public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) { - get + Matrix4 minor = new Matrix4(); + int m = 0, n = 0; + for (int i = 0; i < 4; i++) { - if (!m_isphysical || Body == IntPtr.Zero) - return Vector3.Zero; - - return _torque; - } - - set - { - if (value.IsFinite()) - { - m_taintTorque = value; - _parent_scene.AddPhysicsActorTaint(this); - } - else + if (i == iRow) + continue; + n = 0; + for (int j = 0; j < 4; j++) { - m_log.Warn("[PHYSICS]: Got NaN Torque in Object"); + if (j == iCol) + continue; + Matrix4SetValue(ref minor, m,n, matrix[i, j]); + n++; } + m++; } - } -/* - public Vector3 AngularLock - { - get { return m_angularlock; } - set { } - } - -*/ - public override float CollisionScore - { - get { return m_collisionscore; } - set { m_collisionscore = value; } + return minor; } - public override bool Kinematic + public static Matrix4 Transpose(Matrix4 pMat) { - get { return false; } - set { } + Matrix4 transposeMatrix = new Matrix4(); + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); + return transposeMatrix; } - public override Quaternion Orientation + public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) { - get { return _orientation; } - set + switch (r) { - if (QuaternionIsFinite(value)) - { - _orientation = value; - } - else - m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); + case 0: + switch (c) + { + case 0: + pMat.M11 = val; + break; + case 1: + pMat.M12 = val; + break; + case 2: + pMat.M13 = val; + break; + case 3: + pMat.M14 = val; + break; + } + + break; + case 1: + switch (c) + { + case 0: + pMat.M21 = val; + break; + case 1: + pMat.M22 = val; + break; + case 2: + pMat.M23 = val; + break; + case 3: + pMat.M24 = val; + break; + } + + break; + case 2: + switch (c) + { + case 0: + pMat.M31 = val; + break; + case 1: + pMat.M32 = val; + break; + case 2: + pMat.M33 = val; + break; + case 3: + pMat.M34 = val; + break; + } + + break; + case 3: + switch (c) + { + case 0: + pMat.M41 = val; + break; + case 1: + pMat.M42 = val; + break; + case 2: + pMat.M43 = val; + break; + case 3: + pMat.M44 = val; + break; + } + break; } } + private static float determinant3x3(Matrix4 pMat) + { + float det = 0; + float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; + float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; + float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; + float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; + float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; + float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; - internal static bool QuaternionIsFinite(Quaternion q) + det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); + return det; + + } + + private static void DMassCopy(ref d.Mass src, ref d.Mass dst) { - if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) - return false; - if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) - return false; - if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) - return false; - if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) - return false; - return true; + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; } - public override Vector3 Acceleration + public override void SetMaterial(int pMaterial) { - get { return _acceleration; } + m_material = pMaterial; } + + - public void SetAcceleration(Vector3 accel) - { - _acceleration = accel; - } - public override void AddForce(Vector3 force, bool pushforce) + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) { - if (force.IsFinite()) + switch (pParam) { - lock (m_forcelist) - m_forcelist.Add(force); + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; +// case Vehicle.HOVER_EFFICIENCY: +// if (pValue < 0f) pValue = 0f; +// if (pValue > 1f) pValue = 1f; +// m_VhoverEfficiency = pValue; +// break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable + if (pValue > 1.0f) pValue = 1.0f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue > 30f) pValue = 30f; + if (pValue < 0.1f) pValue = 0.1f; + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + UpdateAngDecay(); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + UpdateLinDecay(); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + break; - m_taintforce = true; - } - else - { - m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object"); } - //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); - } + + }//end ProcessFloatVehicleParam - public override void AddAngularForce(Vector3 force, bool pushforce) + internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) { - if (force.IsFinite()) - { - m_angularforcelist.Add(force); - m_taintaddangularforce = true; - } - else + switch (pParam) { - m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object"); + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X > 30f) pValue.X = 30f; + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y > 30f) pValue.Y = 30f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z > 30f) pValue.Z = 30f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; + if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; + if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; + if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; + if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; + if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; + UpdateAngDecay(); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting + UpdateLinDecay(); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; } - } + + }//end ProcessVectorVehicleParam - public override Vector3 RotationalVelocity + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) { - get - { -/* Vector3 pv = Vector3.Zero; - if (_zeroFlag) - return pv; - m_lastUpdateSent = false; - - if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) - return pv; -*/ - return m_rotationalVelocity; - } - set + switch (pParam) { - if (value.IsFinite()) - { - m_rotationalVelocity = value; - } - else - { - m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object"); - } + case Vehicle.REFERENCE_FRAME: + // m_referenceFrame = pValue; + break; } - } + + }//end ProcessRotationVehicleParam - public override void CrossingFailure() + internal void ProcessFlagsVehicleSet(int flags) { - m_crossingfailures++; - if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - base.RaiseOutOfBounds(_position); - return; - } - else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); - } + m_flags |= (VehicleFlag)flags; } - public override float Buoyancy + internal void ProcessFlagsVehicleRemove(int flags) { - get { return m_buoyancy; } - set { m_buoyancy = value; } + m_flags &= ~((VehicleFlag)flags); } + + internal void ProcessTypeChange(Vehicle pType) + { + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(30, 30, 30); +// m_lLinMotorVel = Vector3.Zero; + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 1; + // m_linearDeflectionTimescale = 1; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 1; + // m_bankingTimescale = 10; + // m_referenceFrame = Quaternion.Identity; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. +// m_lLinMotorVel = Vector3.Zero; + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // // m_linearDeflectionEfficiency = 1; + // // m_linearDeflectionTimescale = 2; + // // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 10f; + // m_bankingEfficiency = -0.2f; + // m_bankingMix = 1; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10,10,10); +// m_lLinMotorVel = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 0.5f; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5f; + // m_bankingEfficiency = -0.3f; + // m_bankingMix = 0.8f; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); +// m_lLinMotorVel = Vector3.Zero; + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + // m_bankingEfficiency = 1; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 2; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10; + m_VhoverHeight = 5; +// m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0; + // m_linearDeflectionTimescale = 5; + // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 100f; + // m_bankingEfficiency = 0; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 5; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; - public override void link(PhysicsActor obj) - { - m_taintparent = obj; - } + } + }//end SetDefaultsForType - public override void delink() + internal void Enable(IntPtr pBody, OdeScene pParentScene) { - m_taintparent = null; - } + if (m_type == Vehicle.TYPE_NONE) + return; - public override void LockAngularMotion(Vector3 axis) + m_body = pBody; + } + + + internal void Halt() + { // Kill all motions, when non-physical + m_linearMotorDirection = Vector3.Zero; + m_lLinMotorDVel = Vector3.Zero; + m_lLinObjectVel = Vector3.Zero; + m_wLinObjectVel = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + m_lastAngularVelocity = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + } + + private void UpdateLinDecay() + { + if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; + if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; + if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; + } // else let the motor decay on its own + + private void UpdateAngDecay() + { + if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; + if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; + if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; + } // else let the motor decay on its own + + public void Move(float timestep) { - // reverse the zero/non zero values for ODE. - if (axis.IsFinite()) - { - axis.X = (axis.X > 0) ? 1f : 0f; - axis.Y = (axis.Y > 0) ? 1f : 0f; - axis.Z = (axis.Z > 0) ? 1f : 0f; - m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - m_taintAngularLock = axis; - } - else - { - m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object"); - } - } + float fx = 0; + float fy = 0; + float fz = 0; - public void UpdatePositionAndVelocity() - { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null) + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; + + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. { - Vector3 pv = Vector3.Zero; - bool lastZeroFlag = _zeroFlag; - if (Body != (IntPtr)0) // FIXME -> or if it is a joint - { + +// Old public void UpdatePositionAndVelocity(), more accuratley calculated here + // Vector3 pv = Vector3.Zero; // what was this for? + bool lastZeroFlag = _zeroFlag; // was it stopped + // if (Body != (IntPtr)0) // FIXME -> or if it is a joint + // { d.Vector3 vec = d.BodyGetPosition(Body); d.Quaternion ori = d.BodyGetQuaternion(Body); d.Vector3 vel = d.BodyGetLinearVel(Body); @@ -2730,12 +3275,13 @@ Console.WriteLine(" JointCreateFixed"); l_orientation.Y = ori.Y; l_orientation.Z = ori.Z; l_orientation.W = ori.W; +//Console.WriteLine("Move {0} at {1}", m_primName, l_position); -// if(l_position.Y != m_lastposition.Y){ -// Console.WriteLine("UP&V {0} {1}", m_primName, l_position); -// } - if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) + if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || + l_position.X < 0f || + l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || + l_position.Y < 0f) { //base.RaiseOutOfBounds(l_position); @@ -2808,7 +3354,7 @@ Console.WriteLine(" JointCreateFixed"); } if (_zeroFlag) - { + { // Its stopped _velocity.X = 0.0f; _velocity.Y = 0.0f; _velocity.Z = 0.0f; @@ -2828,7 +3374,7 @@ Console.WriteLine(" JointCreateFixed"); { m_throttleUpdates = false; throttleCounter = 0; - m_rotationalVelocity = pv; + // m_rotationalVelocity = pv; What was this for? if (_parent == null) { @@ -2839,7 +3385,7 @@ Console.WriteLine(" JointCreateFixed"); } } else - { + { // Its moving if (lastZeroFlag != _zeroFlag) { if (_parent == null) @@ -2855,22 +3401,14 @@ Console.WriteLine(" JointCreateFixed"); _velocity.X = vel.X; _velocity.Y = vel.Y; _velocity.Z = vel.Z; - - _acceleration = ((_velocity - m_lastVelocity) / 0.1f); - _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); - //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); - -// if (_velocity.ApproxEquals(pv, 0.5f)) ???? Disregard rotational vel if lin vel is < 0.5 ????? -// { -// m_rotationalVelocity = pv;/ - -// } -// else -// { - m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); -// } - - //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); +// Why 2 calcs??? +// _acceleration = ((_velocity - m_lastVelocity) / 0.1f); +// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, +// _velocity.Y - m_lastVelocity.Y / 0.1f, +// _velocity.Z - m_lastVelocity.Z / 0.1f); + + _acceleration = ((_velocity - m_lastVelocity) / timestep); + _orientation.X = ori.X; _orientation.Y = ori.Y; _orientation.Z = ori.Z; @@ -2889,434 +3427,592 @@ Console.WriteLine(" JointCreateFixed"); } } m_lastposition = l_position; - } - else - { - // Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; + + /// End of old UpdatePositionAndVelocity insert + +//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration); +//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type + + // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); + if (m_type != Vehicle.TYPE_NONE) + { + // get body attitude + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion irotq = Quaternion.Inverse(rotq); + + // VEHICLE Linear Motion + d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame + Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); + m_lLinObjectVel = vel_now * irotq; + + if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate + { + if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) + { + float decayfactor = m_linearMotorDecayTimescale/timestep; + Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); + m_lLinMotorDVel -= decayAmount; + } + else + { + float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); + Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; + m_lLinMotorDVel -= decel; + } + if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_lLinMotorDVel = Vector3.Zero; + } + else + { + if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; + if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; + if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; + } + } // end linear motor decay + + if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + if (m_linearMotorTimescale < 300.0f) + { + Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; + float linfactor = m_linearMotorTimescale/timestep; + Vector3 attackAmount = (attack_error/linfactor) * 1.3f; + m_lLinObjectVel += attackAmount; + } + if (m_linearFrictionTimescale.X < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.X / timestep; + float fricX = m_lLinObjectVel.X / fricfactor; + m_lLinObjectVel.X -= fricX; + } + if (m_linearFrictionTimescale.Y < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Y / timestep; + float fricY = m_lLinObjectVel.Y / fricfactor; + m_lLinObjectVel.Y -= fricY; + } + if (m_linearFrictionTimescale.Z < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Z / timestep; +//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor); + float fricZ = m_lLinObjectVel.Z / fricfactor; + m_lLinObjectVel.Z -= fricZ; + } + } + m_wLinObjectVel = m_lLinObjectVel * rotq; + + // Gravity and Buoyancy + Vector3 grav = Vector3.Zero; + if(m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force + } // else its 1.0, no gravity. + + // Hovering + if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + +// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped +// m_VhoverTimescale = 0f; // time to acheive height +// timestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; + // Replace Vertical speed with correction figure if significant + if(Math.Abs(herr0) > 0.01f ) + { + //? d.Mass objMass; + //? d.BodyGetMass(Body, out objMass); + m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale); + //KF: m_VhoverEfficiency is not yet implemented + } + else + { + m_wLinObjectVel.Z = 0f; + } + } + else + { // not hovering, Gravity rules + m_wLinObjectVel.Z = vel_now.Z; + } + + + // Vehicle Linear Motion done ======================================= + // Apply velocity + d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); +//if(frcount == 0) Console.WriteLine("Grav {0}", grav); + // end MoveLinear() + + + // MoveAngular + /* + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL + private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL + + private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor + private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body + */ +//if(frcount == 0) Console.WriteLine("MoveAngular "); + + d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); + Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); + angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation + +//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); + + // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. + float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); + m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; + // Decay Angular Motor 2. + if (m_angularMotorDecayTimescale < 300.0f) + { + if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) + { + float decayfactor = (m_angularMotorDecayTimescale)/timestep; + Vector3 decayAmount = (m_angularMotorDVel/decayfactor); + m_angularMotorDVel -= decayAmount; + } + else + { + Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; + m_angularMotorDVel -= decel; + } + + if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_angularMotorDVel = Vector3.Zero; + } + else + { + if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; + if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; + if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; + } + } // end decay angular motor +//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); + +//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if(m_verticalAttractionTimescale < 300) + { + float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + + if (verterr.Z < 0.0f) + { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to + // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. +//Console.WriteLine("InvertFlip"); + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + verterr *= 0.5f; + // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) + + if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) + { + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = - verterr.X; + vertattr.Z = 0f; +//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // scaling appears better usingsquare-law + float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; + float bounce = 1.0f - damped; + // 0 = crit damp, 1 = bouncy + float oavz = angObjectVel.Z; // retain z velocity + // time-scaled correction, which sums, therefore is bouncy: + angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; + // damped, good @ < 90: + angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); + angObjectVel.Z = oavz; +//if(frcount == 0) Console.WriteLine("VA+"); +//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); + } + else + { + // else error is very small + angObjectVel.X = 0f; + angObjectVel.Y = 0f; +//if(frcount == 0) Console.WriteLine("VA0"); + } + } // else vertical attractor is off +//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); + + if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) + { // if motor or object have motion + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + + if (m_angularMotorTimescale < 300.0f) + { + Vector3 attack_error = m_angularMotorDVel - angObjectVel; + float angfactor = m_angularMotorTimescale/timestep; + Vector3 attackAmount = (attack_error/angfactor); + angObjectVel += attackAmount; +//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); +//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); + } + + angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); + angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); + angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); + } // else no signif. motion + +//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); + // Bank section tba + // Deflection section tba +//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); + + m_lastAngularVelocity = angObjectVel; +//if(frcount == 0) Console.WriteLine("angularLock {0}", m_angularLock); + + if (!m_angularLock.ApproxEquals(Vector3.One, 0.003f)) + { + if (m_angularLock.X == 0) + m_lastAngularVelocity.X = 0f; + if (m_angularLock.Y == 0) + m_lastAngularVelocity.Y = 0f; + if (m_angularLock.Z == 0) + m_lastAngularVelocity.Z = 0f; + } + // Apply to the body +// Vector3 aInc = m_lastAngularVelocity - initavel; +//if(frcount == 0) Console.WriteLine("Inc {0}", aInc); + m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation + + d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); +//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); + + } // end VEHICLES #### + else + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 + // NON-'VEHICLES' are dealt with here + // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked. + +// NB this may be wrong - may lock global axis! Should be LOCAL axis! + /// Dynamics Angular Lock ======================================================================== + if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f)) + { + d.Vector3 avel2 = d.BodyGetAngularVel(Body); + if (m_angularlock.X == 0) + avel2.X = 0; + if (m_angularlock.Y == 0) + avel2.Y = 0; + if (m_angularlock.Z == 0) + avel2.Z = 0; + d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); + } - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; - } - } - } + /// Dynamics Buoyancy =============================================================================== + //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // NB Prims in ODE are no subject to global gravity + float m_mass = CalculateMass(); + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass - public override bool FloatOnWater - { - set { - m_taintCollidesWater = value; - _parent_scene.AddPhysicsActorTaint(this); - } - } + if (m_usePID) + { +//if(frcount == 0) Console.WriteLine("PID " + m_primName); + // KF - this is for object MoveToTarget. + + //if (!d.BodyIsEnabled(Body)) + //d.BodySetForce(Body, 0f, 0f, 0f); - public override void SetMomentum(Vector3 momentum) - { - } + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position - public override Vector3 PIDTarget - { - set - { - if (value.IsFinite()) - { - m_PIDTarget = value; - } - else - m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); - } - } - public override bool PIDActive { set { m_usePID = value; } } - public override float PIDTau { set { m_PIDTau = value; } } - - // For RotLookAt - public override Quaternion APIDTarget { set { m_APIDTarget = value; } } - public override bool APIDActive { set { m_useAPID = value; } } - public override float APIDStrength { set { m_APIDStrength = value; } } - public override float APIDDamping { set { m_APIDDamping = value; } } + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + //PidStatus = true; - public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } - public override bool PIDHoverActive { set { m_useHoverPID = value; } } - public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } - public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + // PhysicsVector vec = new PhysicsVector(); +// d.Vector3 vel = d.BodyGetLinearVel(Body); - private void createAMotor(Vector3 axis) // ##* - { -Console.WriteLine(" createAMotor called! ----------------------------"); - if (Body == IntPtr.Zero) - return; + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } + // if velocity is zero, use position control; otherwise, velocity control - float axisnum = 3; + if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions - axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; - // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. - d.Mass objMass; - d.MassSetZero(out objMass); - DMassCopy(ref pMass, ref objMass); + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end if (m_usePID) + + /// Dynamics Hover =================================================================================== + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + if (m_useHoverPID && !m_usePID) + { +//Console.WriteLine("Hover " + m_primName); + + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; - //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); + // no lock; for now it's only called from within Simulate() - Matrix4 dMassMat = FromDMass(objMass); + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position - Matrix4 mathmat = Inverse(dMassMat); + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } - /* - //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]); + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + - mathmat = Inverse(mathmat); - - - objMass = FromMatrix4(mathmat, ref objMass); - //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); - - mathmat = Inverse(mathmat); - */ - if (axis.X == 0) - { - mathmat.M33 = 50.0000001f; - //objMass.I.M22 = 0; - } - if (axis.Y == 0) - { - mathmat.M22 = 50.0000001f; - //objMass.I.M11 = 0; - } - if (axis.Z == 0) - { - mathmat.M11 = 50.0000001f; - //objMass.I.M00 = 0; - } - - - - mathmat = Inverse(mathmat); - objMass = FromMatrix4(mathmat, ref objMass); - //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); - - //return; - if (d.MassCheck(ref objMass)) - { - d.BodySetMass(Body, ref objMass); - } - else - { - //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); - } - - if (axisnum <= 0) - return; - // int dAMotorEuler = 1; - - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - d.JointSetAMotorMode(Amotor, 0); - - d.JointSetAMotorNumAxes(Amotor,(int)axisnum); - int i = 0; - - if (axis.X == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); - i++; - } - - if (axis.Y == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); - i++; - } - - if (axis.Z == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); - i++; - } - - for (int j = 0; j < (int)axisnum; j++) - { - //d.JointSetAMotorAngle(Amotor, j, 0); - } - - //d.JointSetAMotorAngle(Amotor, 1, 0); - //d.JointSetAMotorAngle(Amotor, 2, 0); - - // These lowstops and high stops are effectively (no wiggle room) - d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); - //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f); - d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// - - } - - public Matrix4 FromDMass(d.Mass pMass) - { - Matrix4 obj; - obj.M11 = pMass.I.M00; - obj.M12 = pMass.I.M01; - obj.M13 = pMass.I.M02; - obj.M14 = 0; - obj.M21 = pMass.I.M10; - obj.M22 = pMass.I.M11; - obj.M23 = pMass.I.M12; - obj.M24 = 0; - obj.M31 = pMass.I.M20; - obj.M32 = pMass.I.M21; - obj.M33 = pMass.I.M22; - obj.M34 = 0; - obj.M41 = 0; - obj.M42 = 0; - obj.M43 = 0; - obj.M44 = 1; - return obj; - } - - public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) - { - obj.I.M00 = pMat[0, 0]; - obj.I.M01 = pMat[0, 1]; - obj.I.M02 = pMat[0, 2]; - obj.I.M10 = pMat[1, 0]; - obj.I.M11 = pMat[1, 1]; - obj.I.M12 = pMat[1, 2]; - obj.I.M20 = pMat[2, 0]; - obj.I.M21 = pMat[2, 1]; - obj.I.M22 = pMat[2, 2]; - return obj; - } - - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - _parent_scene.addCollisionEventReporting(this); - } - - public override void UnSubscribeEvents() - { - _parent_scene.remCollisionEventReporting(this); - m_eventsubscription = 0; - } - - public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) - { - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); - CollisionEventsThisFrame.addCollider(CollidedWith, contact); - } - - public void SendCollisions() - { - if (CollisionEventsThisFrame == null) - return; - - base.SendCollisionUpdate(CollisionEventsThisFrame); - - if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) - CollisionEventsThisFrame = null; - else - CollisionEventsThisFrame = new CollisionEventUpdate(); - } - - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); +// d.Vector3 vel = d.BodyGetLinearVel(Body); - public static Matrix4 Inverse(Matrix4 pMat) - { - if (determinant3x3(pMat) == 0) - { - return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible - } + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + } // end switch (m_PIDHoverType) - return (Adjoint(pMat) / determinant3x3(pMat)); - } - public static Matrix4 Adjoint(Matrix4 pMat) - { - Matrix4 adjointMatrix = new Matrix4(); - for (int i=0; i<4; i++) - { - for (int j=0; j<4; j++) - { - Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); - } - } + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); - adjointMatrix = Transpose(adjointMatrix); - return adjointMatrix; - } + // if velocity is zero, use position control; otherwise, velocity control - public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) - { - Matrix4 minor = new Matrix4(); - int m = 0, n = 0; - for (int i = 0; i < 4; i++) - { - if (i == iRow) - continue; - n = 0; - for (int j = 0; j < 4; j++) - { - if (j == iCol) - continue; - Matrix4SetValue(ref minor, m,n, matrix[i, j]); - n++; - } - m++; - } - return minor; - } + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions - public static Matrix4 Transpose(Matrix4 pMat) - { - Matrix4 transposeMatrix = new Matrix4(); - for (int i = 0; i < 4; i++) - for (int j = 0; j < 4; j++) - Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); - return transposeMatrix; - } + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + d.BodyAddForce(Body, 0, 0, fz); + //KF this prevents furthur motions return; + } + else + { + _zeroFlag = false; - public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) - { - switch (r) - { - case 0: - switch (c) - { - case 0: - pMat.M11 = val; - break; - case 1: - pMat.M12 = val; - break; - case 2: - pMat.M13 = val; - break; - case 3: - pMat.M14 = val; - break; - } + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end m_useHoverPID && !m_usePID + + /// Dynamics RotLookAt ================================================================================= + if (m_useAPID) + { + // RotLookAt, apparently overrides all other rotation sources. Inputs: + // Quaternion m_APIDTarget + // float m_APIDStrength // From SL experiments, this is the time to get there + // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly + // Also in SL the mass of the object has no effect on time to get there. + // Factors: +//if(frcount == 0) Console.WriteLine("APID "); + // get present body rotation + float limit = 1.0f; + float scaler = 50f; // adjusts damping time + float RLAservo = 0f; + + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; + float diff_angle; + Vector3 diff_axis; + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); + diff_axis.Normalize(); + if(diff_angle > 0.01f) // diff_angle is always +ve + { +// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if(diff_angle > limit) diff_angle = limit; // cap the rotate rate +// RLAservo = timestep / m_APIDStrength * m_mass * scaler; + // rotforce = rotforce * RLAservo * diff_angle ; + // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); + RLAservo = timestep / m_APIDStrength * scaler; + rotforce = rotforce * RLAservo * diff_angle ; + + if (m_angularlock.X == 0) + rotforce.X = 0; + if (m_angularlock.Y == 0) + rotforce.Y = 0; + if (m_angularlock.Z == 0) + rotforce.Z = 0; + + d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); +//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } +//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); + } // end m_useAPID + + /// Dynamics Apply Forces =================================================================================== + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; - break; - case 1: - switch (c) - { - case 0: - pMat.M21 = val; - break; - case 1: - pMat.M22 = val; - break; - case 2: - pMat.M23 = val; - break; - case 3: - pMat.M24 = val; - break; - } + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; - break; - case 2: - switch (c) - { - case 0: - pMat.M31 = val; - break; - case 1: - pMat.M32 = val; - break; - case 2: - pMat.M33 = val; - break; - case 3: - pMat.M34 = val; - break; - } + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + //m_taintdisable = true; + //base.RaiseOutOfBounds(Position); + //d.BodySetLinearVel(Body, fx, fy, 0f); + if (!d.BodyIsEnabled(Body)) + { + // A physical body at rest on a surface will auto-disable after a while, + // this appears to re-enable it incase the surface it is upon vanishes, + // and the body should fall again. + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); + } - break; - case 3: - switch (c) - { - case 0: - pMat.M41 = val; - break; - case 1: - pMat.M42 = val; - break; - case 2: - pMat.M43 = val; - break; - case 3: - pMat.M44 = val; - break; - } + // 35x10 = 350n times the mass per second applied maximum. + float nmax = 35f * m_mass; + float nmin = -35f * m_mass; - break; + + if (fx > nmax) + fx = nmax; + if (fx < nmin) + fx = nmin; + if (fy > nmax) + fy = nmax; + if (fy < nmin) + fy = nmin; + d.BodyAddForce(Body, fx, fy, fz); +//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + } } - } - private static float determinant3x3(Matrix4 pMat) - { - float det = 0; - float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; - float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; - float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; - float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; - float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; - float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; - - det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); - return det; - - } - - private static void DMassCopy(ref d.Mass src, ref d.Mass dst) - { - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } + else + { // is not physical, or is not a body or is selected + // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; - public override void SetMaterial(int pMaterial) - { - m_material = pMaterial; - } + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; - } + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + return; + } + } // end Move() + } // end class } -- cgit v1.1 From 1379ae4310b94afd1a7a1d9964db26146ca5c7b2 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Fri, 12 Mar 2010 16:13:52 -0500 Subject: Fix Rotation Lock. NOTE: This version had poor border crossing control --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 38 ++++++++++++------------- OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs | 4 --- 2 files changed, 18 insertions(+), 24 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index e51ef5b..367927d 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -64,7 +64,7 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_taintVelocity; private Vector3 m_taintTorque; private Quaternion m_taintrot; - private Vector3 m_angularlock = Vector3.One; // Current setting + private Vector3 m_angularEnable = Vector3.One; // Current setting private Vector3 m_taintAngularLock = Vector3.One; // Request from LSL @@ -216,8 +216,6 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body - - private Vector3 m_angularLock = Vector3.One; //Deflection properties // private float m_angularDeflectionEfficiency = 0; @@ -1401,8 +1399,8 @@ namespace OpenSim.Region.Physics.OdePlugin changefloatonwater(timestep); // ##* - if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) -//Console.WriteLine("ALchange req {0} is {1}", m_taintAngularLock, m_angularlock); + if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f)) +Console.WriteLine("ALchange req {0} is {1}", m_taintAngularLock, m_angularEnable); changeAngularLock(timestep); } @@ -1422,8 +1420,8 @@ namespace OpenSim.Region.Physics.OdePlugin //If we have a parent then we're not authorative here if (_parent == null) { -//Console.WriteLine("Alock changed to {0}", m_taintAngularLock); - m_angularlock = m_taintAngularLock; +Console.WriteLine("Alock changed to {0}", m_taintAngularLock); + m_angularEnable = m_taintAngularLock; } } @@ -3703,15 +3701,15 @@ Console.WriteLine(" JointCreateFixed"); //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); m_lastAngularVelocity = angObjectVel; -//if(frcount == 0) Console.WriteLine("angularLock {0}", m_angularLock); +if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable); - if (!m_angularLock.ApproxEquals(Vector3.One, 0.003f)) + if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) { - if (m_angularLock.X == 0) + if (m_angularEnable.X == 0) m_lastAngularVelocity.X = 0f; - if (m_angularLock.Y == 0) + if (m_angularEnable.Y == 0) m_lastAngularVelocity.Y = 0f; - if (m_angularLock.Z == 0) + if (m_angularEnable.Z == 0) m_lastAngularVelocity.Z = 0f; } // Apply to the body @@ -3727,18 +3725,18 @@ Console.WriteLine(" JointCreateFixed"); { if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 // NON-'VEHICLES' are dealt with here - // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked. + // m_angularEnable = <1,1,1> means no lock. a 0 on axis means locked. // NB this may be wrong - may lock global axis! Should be LOCAL axis! /// Dynamics Angular Lock ======================================================================== - if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f)) + if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) { d.Vector3 avel2 = d.BodyGetAngularVel(Body); - if (m_angularlock.X == 0) + if (m_angularEnable.X == 0) avel2.X = 0; - if (m_angularlock.Y == 0) + if (m_angularEnable.Y == 0) avel2.Y = 0; - if (m_angularlock.Z == 0) + if (m_angularEnable.Z == 0) avel2.Z = 0; d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); } @@ -3938,11 +3936,11 @@ Console.WriteLine(" JointCreateFixed"); RLAservo = timestep / m_APIDStrength * scaler; rotforce = rotforce * RLAservo * diff_angle ; - if (m_angularlock.X == 0) + if (m_angularEnable.X == 0) rotforce.X = 0; - if (m_angularlock.Y == 0) + if (m_angularEnable.Y == 0) rotforce.Y = 0; - if (m_angularlock.Z == 0) + if (m_angularEnable.Z == 0) rotforce.Z = 0; d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs index d1b5b7e..c0e24fd 100644 --- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs +++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs @@ -279,10 +279,6 @@ namespace OpenSim.Region.Physics.PhysXPlugin public override void VehicleFlags(int param, bool remove) { } - public override void VehicleFlagsSet(int param) { } - - public override void VehicleFlagsRemove(int param) { } - public override void SetVolumeDetect(int param) { -- cgit v1.1 From 819806261026cccd68dee649f11938ae5bf10029 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Sun, 14 Mar 2010 16:22:13 -0400 Subject: RotLookAt repaired; debug msg cleanup. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 552 +++++++------------------- 1 file changed, 144 insertions(+), 408 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 367927d..5e6696e 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -652,7 +652,7 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } - public override Vector3 Acceleration + public override Vector3 Acceleration // client updates read data via here { get { return _acceleration; } } @@ -696,14 +696,6 @@ namespace OpenSim.Region.Physics.OdePlugin { get { -/* Vector3 pv = Vector3.Zero; - if (_zeroFlag) - return pv; - m_lastUpdateSent = false; - - if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) - return pv; -*/ return m_rotationalVelocity; } set @@ -1398,9 +1390,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); - // ##* if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f)) -Console.WriteLine("ALchange req {0} is {1}", m_taintAngularLock, m_angularEnable); changeAngularLock(timestep); } @@ -1411,17 +1401,11 @@ Console.WriteLine("ALchange req {0} is {1}", m_taintAngularLock, m_angularEna } - private void changeAngularLock(float timestep) // ##* + private void changeAngularLock(float timestep) { - // do we have a Physical object? -// if (Body != IntPtr.Zero) -// { - //Check that we have a Parent - //If we have a parent then we're not authorative here - if (_parent == null) - { -Console.WriteLine("Alock changed to {0}", m_taintAngularLock); - m_angularEnable = m_taintAngularLock; + if (_parent == null) + { + m_angularEnable = m_taintAngularLock; } } @@ -1435,7 +1419,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock); { OdePrim obj = (OdePrim)m_taintparent; //obj.disableBody(); -//Console.WriteLine("changelink calls ParentPrim"); obj.ParentPrim(this); /* @@ -1453,8 +1436,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock); // destroy link else if (_parent != null && m_taintparent == null) { -//Console.WriteLine(" changelink B"); - if (_parent is OdePrim) { OdePrim obj = (OdePrim)_parent; @@ -1480,7 +1461,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock); // prim is the child public void ParentPrim(OdePrim prim) { -//Console.WriteLine("ParentPrim " + m_primName); if (this.m_localID != prim.m_localID) { if (Body == IntPtr.Zero) @@ -1494,7 +1474,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock); { if (!childrenPrim.Contains(prim)) { -//Console.WriteLine("childrenPrim.Add " + prim); childrenPrim.Add(prim); foreach (OdePrim prm in childrenPrim) @@ -1640,7 +1619,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock); { foreach (OdePrim prm in childrenPrim) { -//Console.WriteLine("ChildSetGeom calls ParentPrim"); ParentPrim(prm); } } @@ -1667,7 +1645,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock); lock (childrenPrim) { - //Console.WriteLine("childrenPrim.Remove " + odePrim); childrenPrim.Remove(odePrim); } @@ -1680,7 +1657,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock); { foreach (OdePrim prm in childrenPrim) { -//Console.WriteLine("ChildDelink calls ParentPrim"); ParentPrim(prm); } } @@ -2417,219 +2393,9 @@ Console.WriteLine(" JointCreateFixed"); public void UpdatePositionAndVelocity() { - return; // moved to the MOVE() method + return; // moved to the Move() method } - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! -/* if (_parent == null) - { - Vector3 pv = Vector3.Zero; - bool lastZeroFlag = _zeroFlag; - if (Body != (IntPtr)0) // FIXME -> or if it is a joint - { - d.Vector3 vec = d.BodyGetPosition(Body); - d.Quaternion ori = d.BodyGetQuaternion(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 rotvel = d.BodyGetAngularVel(Body); - d.Vector3 torque = d.BodyGetTorque(Body); - _torque = new Vector3(torque.X, torque.Y, torque.Z); - Vector3 l_position = Vector3.Zero; - Quaternion l_orientation = Quaternion.Identity; - - // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) - //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - - m_lastposition = _position; - m_lastorientation = _orientation; - - l_position.X = vec.X; - l_position.Y = vec.Y; - l_position.Z = vec.Z; - l_orientation.X = ori.X; - l_orientation.Y = ori.Y; - l_orientation.Z = ori.Z; - l_orientation.W = ori.W; - -// if(l_position.Y != m_lastposition.Y){ -// Console.WriteLine("UP&V {0} {1}", m_primName, l_position); -// } - - if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) - { - //base.RaiseOutOfBounds(l_position); - - if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - _position = l_position; - //_parent_scene.remActivePrim(this); - if (_parent == null) - base.RequestPhysicsterseUpdate(); - return; - } - else - { - if (_parent == null) - base.RaiseOutOfBounds(l_position); - return; - } - } - - if (l_position.Z < 0) - { - // This is so prim that get lost underground don't fall forever and suck up - // - // Sim resources and memory. - // Disables the prim's movement physics.... - // It's a hack and will generate a console message if it fails. - - //IsPhysical = false; - if (_parent == null) - base.RaiseOutOfBounds(_position); - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - if (_parent == null) - base.RequestPhysicsterseUpdate(); - - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - //outofBounds = true; - } - - //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); -//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); - if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) - && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) - && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) -// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) - && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large - { - _zeroFlag = true; -//Console.WriteLine("ZFT 2"); - m_throttleUpdates = false; - } - else - { - //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); - _zeroFlag = false; - m_lastUpdateSent = false; - //m_throttleUpdates = false; - } - - if (_zeroFlag) - { - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - //_orientation.w = 0f; - //_orientation.X = 0f; - //_orientation.Y = 0f; - //_orientation.Z = 0f; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - m_rotationalVelocity = pv; - - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } - - m_lastUpdateSent = true; - } - } - else - { - if (lastZeroFlag != _zeroFlag) - { - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } - } - - m_lastVelocity = _velocity; - - _position = l_position; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - - _acceleration = ((_velocity - m_lastVelocity) / 0.1f); - _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); - //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); - -// if (_velocity.ApproxEquals(pv, 0.5f)) ???? Disregard rotational vel if lin vel is < 0.5 ????? -// { -// m_rotationalVelocity = pv;/ - -// } -// else -// { - m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); -// } - - //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) - { - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } - } - else - { - throttleCounter++; - } - } - m_lastposition = l_position; - } - else - { - // Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; - } - } - } -*/ +/* No one uses this? public Matrix4 FromDMass(d.Mass pMass) { Matrix4 obj; @@ -2651,7 +2417,7 @@ Console.WriteLine(" JointCreateFixed"); obj.M44 = 1; return obj; } - +*/ public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) { obj.I.M00 = pMat[0, 0]; @@ -2878,11 +2644,6 @@ Console.WriteLine(" JointCreateFixed"); m_material = pMaterial; } - - - - - internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) { switch (pParam) @@ -3242,193 +3003,173 @@ Console.WriteLine(" JointCreateFixed"); { // Old public void UpdatePositionAndVelocity(), more accuratley calculated here - // Vector3 pv = Vector3.Zero; // what was this for? - bool lastZeroFlag = _zeroFlag; // was it stopped - // if (Body != (IntPtr)0) // FIXME -> or if it is a joint - // { - d.Vector3 vec = d.BodyGetPosition(Body); - d.Quaternion ori = d.BodyGetQuaternion(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); + bool lastZeroFlag = _zeroFlag; // was it stopped + d.Vector3 vec = d.BodyGetPosition(Body); + d.Quaternion ori = d.BodyGetQuaternion(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); // d.Vector3 rotvel = d.BodyGetAngularVel(Body); - d.Vector3 torque = d.BodyGetTorque(Body); - _torque = new Vector3(torque.X, torque.Y, torque.Z); - Vector3 l_position = Vector3.Zero; - Quaternion l_orientation = Quaternion.Identity; - - // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) - //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - - m_lastposition = _position; - m_lastorientation = _orientation; + d.Vector3 torque = d.BodyGetTorque(Body); + _torque = new Vector3(torque.X, torque.Y, torque.Z); + Vector3 l_position = Vector3.Zero; + Quaternion l_orientation = Quaternion.Identity; + + m_lastposition = _position; + m_lastorientation = _orientation; - l_position.X = vec.X; - l_position.Y = vec.Y; - l_position.Z = vec.Z; - l_orientation.X = ori.X; - l_orientation.Y = ori.Y; - l_orientation.Z = ori.Z; - l_orientation.W = ori.W; + l_position.X = vec.X; + l_position.Y = vec.Y; + l_position.Z = vec.Z; + l_orientation.X = ori.X; + l_orientation.Y = ori.Y; + l_orientation.Z = ori.Z; + l_orientation.W = ori.W; //Console.WriteLine("Move {0} at {1}", m_primName, l_position); + // Check if outside region horizontally + if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || + l_position.X < 0f || + l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || + l_position.Y < 0f) + { + if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { // keep trying to cross? + _position = l_position; + //_parent_scene.remActivePrim(this); + if (_parent == null) base.RequestPhysicsterseUpdate(); + return; // Dont process any other motion? + } + else + { // Too many tries + if (_parent == null) base.RaiseOutOfBounds(l_position); + return; // Dont process any other motion? + } + } // end outside region horizontally - if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || - l_position.X < 0f || - l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || - l_position.Y < 0f) - { - //base.RaiseOutOfBounds(l_position); + if (l_position.Z < 0) + { + // This is so prim that get lost underground don't fall forever and suck up + // + // Sim resources and memory. + // Disables the prim's movement physics.... + // It's a hack and will generate a console message if it fails. - if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - _position = l_position; - //_parent_scene.remActivePrim(this); - if (_parent == null) - base.RequestPhysicsterseUpdate(); - return; - } - else - { - if (_parent == null) - base.RaiseOutOfBounds(l_position); - return; - } - } + //IsPhysical = false; + if (_parent == null) base.RaiseOutOfBounds(_position); + + _acceleration.X = 0; // This stuff may stop client display but it has no + _acceleration.Y = 0; // effect on the object in phys engine! + _acceleration.Z = 0; - if (l_position.Z < 0) - { - // This is so prim that get lost underground don't fall forever and suck up - // - // Sim resources and memory. - // Disables the prim's movement physics.... - // It's a hack and will generate a console message if it fails. + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; - //IsPhysical = false; - if (_parent == null) - base.RaiseOutOfBounds(_position); + if (_parent == null) base.RequestPhysicsterseUpdate(); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + //outofBounds = true; + } // end neg Z check - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; + // Is it moving? + if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) + && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large + { + _zeroFlag = true; +//Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } - if (_parent == null) - base.RequestPhysicsterseUpdate(); + if (_zeroFlag) + { // Its stopped + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + //_orientation.w = 0f; + //_orientation.X = 0f; + //_orientation.Y = 0f; + //_orientation.Z = 0f; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { m_throttleUpdates = false; throttleCounter = 0; - _zeroFlag = true; - //outofBounds = true; - } + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } - //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); -//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); - if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) - && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) - && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) -// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) - && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large - { - _zeroFlag = true; -//Console.WriteLine("ZFT 2"); - m_throttleUpdates = false; + m_lastUpdateSent = true; } - else + } + else + { // Its moving + if (lastZeroFlag != _zeroFlag) { - //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); - _zeroFlag = false; - m_lastUpdateSent = false; - //m_throttleUpdates = false; - } - - if (_zeroFlag) - { // Its stopped - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - //_orientation.w = 0f; - //_orientation.X = 0f; - //_orientation.Y = 0f; - //_orientation.Z = 0f; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - if (!m_lastUpdateSent) + if (_parent == null) { - m_throttleUpdates = false; - throttleCounter = 0; - // m_rotationalVelocity = pv; What was this for? - - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } - - m_lastUpdateSent = true; + base.RequestPhysicsterseUpdate(); } } - else - { // Its moving - if (lastZeroFlag != _zeroFlag) - { - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } - } - m_lastVelocity = _velocity; + m_lastVelocity = _velocity; - _position = l_position; + _position = l_position; - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; // Why 2 calcs??? -// _acceleration = ((_velocity - m_lastVelocity) / 0.1f); -// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, -// _velocity.Y - m_lastVelocity.Y / 0.1f, +// _acceleration = ((_velocity - m_lastVelocity) / 0.1f); +// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, +// _velocity.Y - m_lastVelocity.Y / 0.1f, // _velocity.Z - m_lastVelocity.Z / 0.1f); - _acceleration = ((_velocity - m_lastVelocity) / timestep); + _acceleration = ((_velocity - m_lastVelocity) / timestep); - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) - { - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } - } - else + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + if (_parent == null) { - throttleCounter++; + base.RequestPhysicsterseUpdate(); } } - m_lastposition = l_position; - + else + { + throttleCounter++; + } + } + m_lastposition = l_position; + /// End of old UpdatePositionAndVelocity insert //if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration); -//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type + - // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); +// if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type + +// " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); if (m_type != Vehicle.TYPE_NONE) { // get body attitude @@ -3701,7 +3442,6 @@ Console.WriteLine(" JointCreateFixed"); //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); m_lastAngularVelocity = angObjectVel; -if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable); if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) { @@ -3720,14 +3460,11 @@ if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable); d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); - } // end VEHICLES #### + } // end VEHICLES else { if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 // NON-'VEHICLES' are dealt with here - // m_angularEnable = <1,1,1> means no lock. a 0 on axis means locked. - -// NB this may be wrong - may lock global axis! Should be LOCAL axis! /// Dynamics Angular Lock ======================================================================== if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) { @@ -3802,7 +3539,7 @@ if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable); d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); d.BodySetLinearVel(Body, 0, 0, 0); d.BodyAddForce(Body, 0, 0, fz); - return; + // return; } else { @@ -3911,7 +3648,6 @@ if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable); // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly // Also in SL the mass of the object has no effect on time to get there. // Factors: -//if(frcount == 0) Console.WriteLine("APID "); // get present body rotation float limit = 1.0f; float scaler = 50f; // adjusts damping time -- cgit v1.1 From add47befacb0397f2a0b5c9455ed1e658616aba4 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Fri, 19 Mar 2010 18:53:16 -0400 Subject: Update acceleration to 0 on no tphysical. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 5e6696e..e8894f7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -804,6 +804,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical && Body != IntPtr.Zero) { d.BodyDisable(Body); + Halt(); } } @@ -2972,7 +2973,8 @@ Console.WriteLine(" JointCreateFixed"); m_wLinObjectVel = Vector3.Zero; m_angularMotorDirection = Vector3.Zero; m_lastAngularVelocity = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + _acceleration = Vector3.Zero; } private void UpdateLinDecay() -- cgit v1.1 From e2a521742a737aa54e43dd934c0a0d5f2e380e8f Mon Sep 17 00:00:00 2001 From: kitto Date: Sun, 4 Apr 2010 22:57:32 -0400 Subject: ChODE Only: Corrected Heightmap load to iliminate crack at 255M. Added 'fence' option to prevent physical objects crossing region border. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 202 +++++++++++++----------- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 111 ++++++------- 2 files changed, 157 insertions(+), 156 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index e8894f7..67f8af5 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -180,6 +180,7 @@ namespace OpenSim.Region.Physics.OdePlugin internal int m_material = (int)Material.Wood; private int frcount = 0; // Used to limit dynamics debug output to + private int revcount = 0; // Reverse motion while > 0 private IntPtr m_body = IntPtr.Zero; @@ -1888,22 +1889,19 @@ namespace OpenSim.Region.Physics.OdePlugin public void changemove(float timestep) { -//Console.WriteLine("changemove for {0}", m_primName ); - +//Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position ); if (m_isphysical) { //Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim); // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! if (!m_taintremove && !childPrim) { -//Console.WriteLine("physOK"); if (Body == IntPtr.Zero) enableBody(); //Prim auto disable after 20 frames, //if you move it, re-enable the prim manually. if (_parent != null) { -//Console.WriteLine("physChild"); if (m_linkJoint != IntPtr.Zero) { d.JointDestroy(m_linkJoint); @@ -1912,7 +1910,6 @@ namespace OpenSim.Region.Physics.OdePlugin } if (Body != IntPtr.Zero) { -//Console.WriteLine("physNotIPZ"); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); if (_parent != null) @@ -1945,7 +1942,6 @@ Console.WriteLine(" JointCreateFixed"); } else { -//Console.WriteLine("NONphys"); // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); _parent_scene.waitForSpaceUnlock(m_targetSpace); @@ -2382,9 +2378,7 @@ Console.WriteLine(" JointCreateFixed"); if (IsPhysical) { if (Body != IntPtr.Zero) - { d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); - } } //resetCollisionAccounting(); @@ -2394,31 +2388,9 @@ Console.WriteLine(" JointCreateFixed"); public void UpdatePositionAndVelocity() { - return; // moved to the Move() method - } -/* No one uses this? - public Matrix4 FromDMass(d.Mass pMass) - { - Matrix4 obj; - obj.M11 = pMass.I.M00; - obj.M12 = pMass.I.M01; - obj.M13 = pMass.I.M02; - obj.M14 = 0; - obj.M21 = pMass.I.M10; - obj.M22 = pMass.I.M11; - obj.M23 = pMass.I.M12; - obj.M24 = 0; - obj.M31 = pMass.I.M20; - obj.M32 = pMass.I.M21; - obj.M33 = pMass.I.M22; - obj.M34 = 0; - obj.M41 = 0; - obj.M42 = 0; - obj.M43 = 0; - obj.M44 = 1; - return obj; + return; // moved to the Move () method } -*/ + public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) { obj.I.M00 = pMat[0, 0]; @@ -2996,44 +2968,114 @@ Console.WriteLine(" JointCreateFixed"); float fx = 0; float fy = 0; float fz = 0; + Vector3 linvel; // velocity applied, including any reversal + int outside = 0; + + // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. + // This is a temp patch until proper region crossing is developed. + + int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; + int fence = _parent_scene.geomRegionFence; + + float border_limit = 0.05f; // original limit + if (fence == 1) border_limit = 0.5f; // bounce point frcount++; // used to limit debug comment output if (frcount > 100) frcount = 0; - if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. + if(revcount > 0) revcount--; + + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims. { - -// Old public void UpdatePositionAndVelocity(), more accuratley calculated here + // Old public void UpdatePositionAndVelocity(), more accuratley calculated here bool lastZeroFlag = _zeroFlag; // was it stopped + d.Vector3 vec = d.BodyGetPosition(Body); - d.Quaternion ori = d.BodyGetQuaternion(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - // d.Vector3 rotvel = d.BodyGetAngularVel(Body); - d.Vector3 torque = d.BodyGetTorque(Body); - _torque = new Vector3(torque.X, torque.Y, torque.Z); Vector3 l_position = Vector3.Zero; - Quaternion l_orientation = Quaternion.Identity; - - m_lastposition = _position; - m_lastorientation = _orientation; - l_position.X = vec.X; l_position.Y = vec.Y; l_position.Z = vec.Z; - l_orientation.X = ori.X; - l_orientation.Y = ori.Y; - l_orientation.Z = ori.Z; - l_orientation.W = ori.W; + m_lastposition = _position; + _position = l_position; + + d.Quaternion ori = d.BodyGetQuaternion(Body); + // Quaternion l_orientation = Quaternion.Identity; + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + m_lastorientation = _orientation; + + d.Vector3 vel = d.BodyGetLinearVel(Body); + m_lastVelocity = _velocity; + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + _acceleration = ((_velocity - m_lastVelocity) / timestep); + + d.Vector3 torque = d.BodyGetTorque(Body); + _torque = new Vector3(torque.X, torque.Y, torque.Z); + + base.RequestPhysicsterseUpdate(); + //Console.WriteLine("Move {0} at {1}", m_primName, l_position); - // Check if outside region horizontally - if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || - l_position.X < 0f || - l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || - l_position.Y < 0f) + // Check if outside region + // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! + if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit)) + { + l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit); + outside = 1; + } + + if (l_position.X < border_limit) + { + l_position.X = border_limit; + outside = 2; + } + if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit)) + { + l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit); + outside = 3; + } + + if (l_position.Y < border_limit) + { + l_position.Y = border_limit; + outside = 4; + } + + if (outside > 0) { - if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) +//Console.WriteLine(" fence = {0}",fence); + +//Console.WriteLine("Border {0}", l_position); + if (fence == 1) // bounce object off boundary + { + if (revcount == 0) + { + if (outside < 3) + { + _velocity.X = -_velocity.X; + } + else + { + _velocity.Y = -_velocity.Y; + } + if (m_type != Vehicle.TYPE_NONE) Halt(); + _position = l_position; + m_taintposition = _position; + m_lastVelocity = _velocity; + _acceleration = Vector3.Zero; + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + base.RequestPhysicsterseUpdate(); + + revcount = 25; // wait for object to move away from border + } + } // else old crossing mode + else if (m_crossingfailures < failureLimit) { // keep trying to cross? _position = l_position; //_parent_scene.remActivePrim(this); @@ -3043,9 +3085,12 @@ Console.WriteLine(" JointCreateFixed"); else { // Too many tries if (_parent == null) base.RaiseOutOfBounds(l_position); +//Console.WriteLine("ROOB 2"); + return; // Dont process any other motion? - } + } // end various methods } // end outside region horizontally + if (l_position.Z < 0) { @@ -3057,6 +3102,8 @@ Console.WriteLine(" JointCreateFixed"); //IsPhysical = false; if (_parent == null) base.RaiseOutOfBounds(_position); +//Console.WriteLine("ROOB 3"); + _acceleration.X = 0; // This stuff may stop client display but it has no _acceleration.Y = 0; // effect on the object in phys engine! @@ -3081,10 +3128,9 @@ Console.WriteLine(" JointCreateFixed"); if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) - && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large + && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // KF 0.01 is far to large { _zeroFlag = true; -//Console.WriteLine("ZFT 2"); m_throttleUpdates = false; } else @@ -3104,10 +3150,7 @@ Console.WriteLine(" JointCreateFixed"); _acceleration.X = 0; _acceleration.Y = 0; _acceleration.Z = 0; - //_orientation.w = 0f; - //_orientation.X = 0f; - //_orientation.Y = 0f; - //_orientation.Z = 0f; + m_rotationalVelocity.X = 0; m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; @@ -3132,26 +3175,6 @@ Console.WriteLine(" JointCreateFixed"); base.RequestPhysicsterseUpdate(); } } - - m_lastVelocity = _velocity; - - _position = l_position; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; -// Why 2 calcs??? -// _acceleration = ((_velocity - m_lastVelocity) / 0.1f); -// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, -// _velocity.Y - m_lastVelocity.Y / 0.1f, -// _velocity.Z - m_lastVelocity.Z / 0.1f); - - _acceleration = ((_velocity - m_lastVelocity) / timestep); - - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; m_lastUpdateSent = false; if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) { @@ -3167,11 +3190,8 @@ Console.WriteLine(" JointCreateFixed"); } m_lastposition = l_position; - /// End of old UpdatePositionAndVelocity insert + /// End UpdatePositionAndVelocity insert -//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration); -// if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type + -// " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); if (m_type != Vehicle.TYPE_NONE) { // get body attitude @@ -3299,11 +3319,12 @@ Console.WriteLine(" JointCreateFixed"); { // not hovering, Gravity rules m_wLinObjectVel.Z = vel_now.Z; } - - + linvel = m_wLinObjectVel; + // Vehicle Linear Motion done ======================================= // Apply velocity - d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z); +//if(frcount == 0) Console.WriteLine("LV {0}", linvel); + d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); // apply gravity force d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); //if(frcount == 0) Console.WriteLine("Grav {0}", grav); @@ -3626,9 +3647,10 @@ Console.WriteLine(" JointCreateFixed"); // react to the physics scene by moving it's position. // Avatar to Avatar collisions // Prim to avatar collisions + d.Vector3 dlinvel = vel; d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); d.BodyAddForce(Body, 0, 0, fz); //KF this prevents furthur motions return; } diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index deb6164..79e2986 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -224,6 +224,7 @@ namespace OpenSim.Region.Physics.OdePlugin public float bodyPIDG = 25; public int geomCrossingFailuresBeforeOutofbounds = 5; + public int geomRegionFence = 0; public float bodyMotorJointMaxforceTensor = 2; @@ -447,6 +448,7 @@ namespace OpenSim.Region.Physics.OdePlugin geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); + geomRegionFence = physicsconfig.GetInt("region_border_fence", 0); geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); @@ -3419,76 +3421,56 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetTerrain(float[] heightMap, Vector3 pOffset) { - // this._heightmap[i] = (double)heightMap[i]; - // dbm (danx0r) -- creating a buffer zone of one extra sample all around - //_origheightmap = heightMap; + uint regionsize = (uint) Constants.RegionSize; // visible region size eg. 256(M) + + uint heightmapWidth = regionsize + 1; // ODE map size 257 x 257 (Meters) (1 extra + uint heightmapHeight = regionsize + 1; + + uint heightmapWidthSamples = (uint)regionsize + 2; // Sample file size, 258 x 258 samples + uint heightmapHeightSamples = (uint)regionsize + 2; + + // Array of height samples for ODE float[] _heightmap; + _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258 - // zero out a heightmap array float array (single dimension [flattened])) - //if ((int)Constants.RegionSize == 256) - // _heightmap = new float[514 * 514]; - //else - - _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; - - uint heightmapWidth = Constants.RegionSize + 1; - uint heightmapHeight = Constants.RegionSize + 1; - - uint heightmapWidthSamples; - - uint heightmapHeightSamples; - - //if (((int)Constants.RegionSize) == 256) - //{ - // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2; - // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2; - // heightmapWidth++; - // heightmapHeight++; - //} - //else - //{ - - heightmapWidthSamples = (uint)Constants.RegionSize + 1; - heightmapHeightSamples = (uint)Constants.RegionSize + 1; - //} - + // Other ODE parameters const float scale = 1.0f; const float offset = 0.0f; - const float thickness = 0.2f; + const float thickness = 2.0f; // Was 0.2f, Larger appears to prevent Av fall-through const int wrap = 0; - int regionsize = (int) Constants.RegionSize + 2; - //Double resolution - //if (((int)Constants.RegionSize) == 256) - // heightMap = ResizeTerrain512Interpolation(heightMap); - - - // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256) - // regionsize = 512; - - float hfmin = 2000; - float hfmax = -2000; + float hfmin = 2000f; + float hfmax = -2000f; + float minele = 0.0f; // Dont allow -ve heights - for (int x = 0; x < heightmapWidthSamples; x++) + uint x = 0; + uint y = 0; + uint xx = 0; + uint yy = 0; + + // load the height samples array from the heightMap + for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257 + { + for ( y = 0; y < heightmapHeightSamples; y++) // 0 to 257 { - for (int y = 0; y < heightmapHeightSamples; y++) - { - int xx = Util.Clip(x - 1, 0, regionsize - 1); - int yy = Util.Clip(y - 1, 0, regionsize - 1); - - - float val= heightMap[yy * (int)Constants.RegionSize + xx]; - _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val; - - hfmin = (val < hfmin) ? val : hfmin; - hfmax = (val > hfmax) ? val : hfmax; - } + xx = x - 1; + if (xx < 0) xx = 0; + if (xx > (regionsize - 1)) xx = regionsize - 1; + + yy = y - 1; + if (yy < 0) yy = 0; + if (yy > (regionsize - 1)) yy = regionsize - 1; + // Input xx = 0 0 1 2 ..... 254 255 255 256 total in + // Output x = 0 1 2 3 ..... 255 256 257 258 total out + float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255> + if (val < minele) val = minele; + _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> + hfmin = (val < hfmin) ? val : hfmin; + hfmax = (val > hfmax) ? val : hfmax; } + } - - - lock (OdeLock) { IntPtr GroundGeom = IntPtr.Zero; @@ -3504,19 +3486,17 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceRemove(space, GroundGeom); d.GeomDestroy(GroundGeom); } - } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1, - (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale, - offset, thickness, wrap); + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, + heightmapWidth, heightmapHeight, (int)heightmapWidthSamples, + (int)heightmapHeightSamples, scale, offset, thickness, wrap); d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); GroundGeom = d.CreateHeightfield(space, HeightmapData, 1); if (GroundGeom != IntPtr.Zero) { d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); - } geom_name_map[GroundGeom] = "Terrain"; @@ -3534,7 +3514,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)) - 1, (pOffset.Y + ((int)Constants.RegionSize * 0.5f)) - 1, 0); + d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0); IntPtr testGround = IntPtr.Zero; if (RegionTerrain.TryGetValue(pOffset, out testGround)) { @@ -3542,7 +3522,6 @@ namespace OpenSim.Region.Physics.OdePlugin } RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); TerrainHeightFieldHeights.Add(GroundGeom,_heightmap); - } } -- cgit v1.1 From 9b6e89457c692fd65a0a221ef7809cc0e4836fe7 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Sun, 11 Apr 2010 14:12:01 -0400 Subject: Fixed LINEAR_MOTOR Z drive, Mantis #30 --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 58 ++++++++++++++++----------- 1 file changed, 35 insertions(+), 23 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 67f8af5..01c3f81 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -2626,15 +2626,15 @@ Console.WriteLine(" JointCreateFixed"); // m_angularDeflectionEfficiency = pValue; break; case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; + if (pValue < 0.1f) pValue = 0.1f; // m_angularDeflectionTimescale = pValue; break; case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; + if (pValue < 0.3f) pValue = 0.3f; m_angularMotorDecayTimescale = pValue; break; case Vehicle.ANGULAR_MOTOR_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; + if (pValue < 0.3f) pValue = 0.3f; m_angularMotorTimescale = pValue; break; case Vehicle.BANKING_EFFICIENCY: @@ -2663,7 +2663,7 @@ Console.WriteLine(" JointCreateFixed"); m_VhoverHeight = pValue; break; case Vehicle.HOVER_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; + if (pValue < 0.1f) pValue = 0.1f; m_VhoverTimescale = pValue; break; case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: @@ -2675,11 +2675,11 @@ Console.WriteLine(" JointCreateFixed"); // m_linearDeflectionTimescale = pValue; break; case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; + if (pValue < 0.3f) pValue = 0.3f; m_linearMotorDecayTimescale = pValue; break; case Vehicle.LINEAR_MOTOR_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; + if (pValue < 0.1f) pValue = 0.1f; m_linearMotorTimescale = pValue; break; case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: @@ -2688,7 +2688,7 @@ Console.WriteLine(" JointCreateFixed"); m_verticalAttractionEfficiency = pValue; break; case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; + if (pValue < 0.1f) pValue = 0.1f; m_verticalAttractionTimescale = pValue; break; @@ -2704,6 +2704,7 @@ Console.WriteLine(" JointCreateFixed"); UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue < 0.1f) pValue = 0.1f; m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -2743,6 +2744,9 @@ Console.WriteLine(" JointCreateFixed"); UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -2939,7 +2943,7 @@ Console.WriteLine(" JointCreateFixed"); internal void Halt() { // Kill all motions, when non-physical - m_linearMotorDirection = Vector3.Zero; + // m_linearMotorDirection = Vector3.Zero; m_lLinMotorDVel = Vector3.Zero; m_lLinObjectVel = Vector3.Zero; m_wLinObjectVel = Vector3.Zero; @@ -2951,16 +2955,22 @@ Console.WriteLine(" JointCreateFixed"); private void UpdateLinDecay() { - if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; - if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; - if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; +// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; +// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; +// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; + m_lLinMotorDVel.X = m_linearMotorDirection.X; + m_lLinMotorDVel.Y = m_linearMotorDirection.Y; + m_lLinMotorDVel.Z = m_linearMotorDirection.Z; } // else let the motor decay on its own private void UpdateAngDecay() { - if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; - if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; - if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; +// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; +// if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; +// if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; + m_angularMotorDVel.X = m_angularMotorDirection.X; + m_angularMotorDVel.Y = m_angularMotorDirection.Y; + m_angularMotorDVel.Z = m_angularMotorDirection.Z; } // else let the motor decay on its own public void Move(float timestep) @@ -2981,7 +2991,7 @@ Console.WriteLine(" JointCreateFixed"); if (fence == 1) border_limit = 0.5f; // bounce point frcount++; // used to limit debug comment output - if (frcount > 100) + if (frcount > 10) frcount = 0; if(revcount > 0) revcount--; @@ -3222,12 +3232,13 @@ Console.WriteLine(" JointCreateFixed"); { m_lLinMotorDVel = Vector3.Zero; } - else + + /* else { if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; - if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; - } + if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; + } */ } // end linear motor decay if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) @@ -3255,7 +3266,6 @@ Console.WriteLine(" JointCreateFixed"); if (m_linearFrictionTimescale.Z < 300.0f) { float fricfactor = m_linearFrictionTimescale.Z / timestep; -//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor); float fricZ = m_lLinObjectVel.Z / fricfactor; m_lLinObjectVel.Z -= fricZ; } @@ -3316,15 +3326,17 @@ Console.WriteLine(" JointCreateFixed"); } } else - { // not hovering, Gravity rules - m_wLinObjectVel.Z = vel_now.Z; + { // not hovering + if (m_wLinObjectVel.Z == 0f) + { // Gravity rules + m_wLinObjectVel.Z = vel_now.Z; + } // else the motor has it } linvel = m_wLinObjectVel; // Vehicle Linear Motion done ======================================= // Apply velocity -//if(frcount == 0) Console.WriteLine("LV {0}", linvel); - d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); + d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); // apply gravity force d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); //if(frcount == 0) Console.WriteLine("Grav {0}", grav); -- cgit v1.1 From 5c23b4aa56a9de1baceb833ecba67f66274dca49 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 29 Apr 2010 13:50:11 -0400 Subject: Fix vertical attractor --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 84 ++++++++++++++------------- 1 file changed, 43 insertions(+), 41 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 01c3f81..fbe52c8 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -3395,6 +3395,44 @@ Console.WriteLine(" JointCreateFixed"); //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); + if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) + { // if motor or object have motion + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + + if (m_angularMotorTimescale < 300.0f) + { + Vector3 attack_error = m_angularMotorDVel - angObjectVel; + float angfactor = m_angularMotorTimescale/timestep; + Vector3 attackAmount = (attack_error/angfactor); + angObjectVel += attackAmount; +//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); +//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); + } + + angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); + angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); + angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); + } // else no signif. motion + +//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); + // Bank section tba + // Deflection section tba +//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); + + + // Rotation Axis Disables: + if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) + { + if (m_angularEnable.X == 0) + angObjectVel.X = 0f; + if (m_angularEnable.Y == 0) + angObjectVel.Y = 0f; + if (m_angularEnable.Z == 0) + angObjectVel.Z = 0f; + } + + angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation + // Vertical attractor section Vector3 vertattr = Vector3.Zero; @@ -3419,8 +3457,9 @@ Console.WriteLine(" JointCreateFixed"); } verterr *= 0.5f; // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) - - if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) + Vector3 xyav = angObjectVel; + xyav.Z = 0.0f; + if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) { // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. @@ -3452,46 +3491,9 @@ Console.WriteLine(" JointCreateFixed"); } // else vertical attractor is off //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); - if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { // if motor or object have motion - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - - if (m_angularMotorTimescale < 300.0f) - { - Vector3 attack_error = m_angularMotorDVel - angObjectVel; - float angfactor = m_angularMotorTimescale/timestep; - Vector3 attackAmount = (attack_error/angfactor); - angObjectVel += attackAmount; -//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); -//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); - } - - angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); - angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); - angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); - } // else no signif. motion - -//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); - // Bank section tba - // Deflection section tba -//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); - + m_lastAngularVelocity = angObjectVel; - - if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) - { - if (m_angularEnable.X == 0) - m_lastAngularVelocity.X = 0f; - if (m_angularEnable.Y == 0) - m_lastAngularVelocity.Y = 0f; - if (m_angularEnable.Z == 0) - m_lastAngularVelocity.Z = 0f; - } - // Apply to the body -// Vector3 aInc = m_lastAngularVelocity - initavel; -//if(frcount == 0) Console.WriteLine("Inc {0}", aInc); - m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation - + // apply Angular Velocity to body d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); -- cgit v1.1 From f3c21524be94dac139e15b0cd80b1e79570f7916 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 29 Apr 2010 19:41:08 -0400 Subject: Fix Av jump motion S/W --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 8e87ad9..7a5093b 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -1025,11 +1025,11 @@ namespace OpenSim.Region.Physics.OdePlugin // m_iscolliding includes collisions with the ground. // d.Vector3 pos = d.BodyGetPosition(Body); - if (_target_velocity.X > 0) + if (Math.Abs(_target_velocity.X) > 0) { vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D; } - if (_target_velocity.Y > 0) + if (Math.Abs(_target_velocity.Y) > 0) { vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D; } -- cgit v1.1 From add7abc1de2fce8db4c6d01cc4b5305bafa4bd87 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Fri, 7 May 2010 14:12:07 -0400 Subject: Fix Mouse+WASD makes Av rise; Fix PREJUMP. --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 30 +++++++++++++++------- 1 file changed, 21 insertions(+), 9 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 7a5093b..38c38b6 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -142,7 +142,7 @@ namespace OpenSim.Region.Physics.OdePlugin // unique UUID of this character object public UUID m_uuid; public bool bad = false; - + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor) { m_uuid = UUID.Random(); @@ -892,7 +892,7 @@ namespace OpenSim.Region.Physics.OdePlugin // If the PID Controller isn't active then we set our force // calculating base velocity to the current position - + if (Body == IntPtr.Zero) return; @@ -972,8 +972,17 @@ namespace OpenSim.Region.Physics.OdePlugin // Prim to avatar collisions d.Vector3 pos = d.BodyGetPosition(Body); - vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2); + float errX = _zeroPosition.X - pos.X; + float errY = _zeroPosition.Y - pos.Y; + if( (Math.Abs(errX) > 0.1f) || (Math.Abs(errY) > 0.1f) ) + { + vec.X = (_target_velocity.X - vel.X) * (PID_D) + (errX) * (PID_P * 2); + vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (errY) * (PID_P * 2); + } + else + { // close, jump to lateral destination + d.BodySetPosition(Body, _zeroPosition.X, _zeroPosition.Y, pos.Z); + } if (flying) { vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; @@ -1056,11 +1065,14 @@ namespace OpenSim.Region.Physics.OdePlugin } if (vec.IsFinite()) { - doForce(vec); - if (!_zeroFlag) - { - AlignAvatarTiltWithCurrentDirectionOfMovement(vec); - } + if (!vec.ApproxEquals(Vector3.Zero, 0.01f)) + { + doForce(vec); + if (!_zeroFlag) + { + AlignAvatarTiltWithCurrentDirectionOfMovement(vec); + } + } } else { -- cgit v1.1 From 6e2cc98eacf48a24ae19433a3f6c2ba9693c5b9e Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Tue, 8 Jun 2010 16:15:16 -0400 Subject: Fixed llMoveToTarget() and Rotation Lock/Enable (ChODE/ODEPrim only) --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 230 ++++++++++++++++++++------ 1 file changed, 179 insertions(+), 51 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index fbe52c8..7ce01dc 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -64,11 +64,16 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_taintVelocity; private Vector3 m_taintTorque; private Quaternion m_taintrot; - private Vector3 m_angularEnable = Vector3.One; // Current setting - private Vector3 m_taintAngularLock = Vector3.One; // Request from LSL - - + private Vector3 m_rotateEnable = Vector3.One; // Current setting + private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL + private bool m_rotateEnableUpdate = false; + private Vector3 m_lockX; + private Vector3 m_lockY; + private Vector3 m_lockZ; private IntPtr Amotor = IntPtr.Zero; + private IntPtr AmotorX = IntPtr.Zero; + private IntPtr AmotorY = IntPtr.Zero; + private IntPtr AmotorZ = IntPtr.Zero; private Vector3 m_PIDTarget; private float m_PIDTau; @@ -729,7 +734,12 @@ namespace OpenSim.Region.Physics.OdePlugin public override float Buoyancy { get { return m_buoyancy; } - set { m_buoyancy = value; } +// set { m_buoyancy = value; } + set { + m_buoyancy = value; + + Console.WriteLine("m_buoyancy={0}", m_buoyancy); + } } public override void link(PhysicsActor obj) @@ -744,14 +754,18 @@ namespace OpenSim.Region.Physics.OdePlugin public override void LockAngularMotion(Vector3 axis) { - // reverse the zero/non zero values for ODE. + // This is actually ROTATION ENABLE, not a lock. + // default is <1,1,1> which is all enabled. + // The lock value is updated inside Move(), no point in using the taint system. + // OS 'm_taintAngularLock' etc change to m_rotateEnable. if (axis.IsFinite()) { axis.X = (axis.X > 0) ? 1f : 0f; axis.Y = (axis.Y > 0) ? 1f : 0f; axis.Z = (axis.Z > 0) ? 1f : 0f; m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - m_taintAngularLock = axis; + m_rotateEnableRequest = axis; + m_rotateEnableUpdate = true; } else { @@ -1391,10 +1405,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); - - if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f)) +/* obsolete + if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) changeAngularLock(timestep); - + */ } else { @@ -1402,15 +1416,16 @@ namespace OpenSim.Region.Physics.OdePlugin } } - +/* obsolete private void changeAngularLock(float timestep) { if (_parent == null) { - m_angularEnable = m_taintAngularLock; + m_angularLock = m_taintAngularLock; + m_angularLockSet = true; } } - + */ private void changelink(float timestep) { // If the newly set parent is not null @@ -2991,7 +3006,7 @@ Console.WriteLine(" JointCreateFixed"); if (fence == 1) border_limit = 0.5f; // bounce point frcount++; // used to limit debug comment output - if (frcount > 10) + if (frcount > 50) frcount = 0; if(revcount > 0) revcount--; @@ -3200,8 +3215,91 @@ Console.WriteLine(" JointCreateFixed"); } m_lastposition = l_position; - /// End UpdatePositionAndVelocity insert - + /// End UpdatePositionAndVelocity insert + + + // Rotation lock ===================================== + if(m_rotateEnableUpdate) + { + // Snapshot current angles, set up Amotor(s) + m_rotateEnableUpdate = false; + m_rotateEnable = m_rotateEnableRequest; +Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; +Console.WriteLine("Old Amotor Destroyed"); + } + + if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) + { // not all are enabled + d.Quaternion r = d.BodyGetQuaternion(Body); + Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); + // extract the axes vectors + Vector3 vX = new Vector3(1f,0f,0f); + Vector3 vY = new Vector3(0f,1f,0f); + Vector3 vZ = new Vector3(0f,0f,1f); + vX = vX * locrot; + vY = vY * locrot; + vZ = vZ * locrot; + // snapshot the current angle vectors + m_lockX = vX; + m_lockY = vY; + m_lockZ = vZ; + // m_lockRot = locrot; + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + d.JointSetAMotorMode(Amotor, 0); // User mode?? +Console.WriteLine("New Amotor Created for {0}", m_primName); + + float axisnum = 3; // how many to lock + axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); + d.JointSetAMotorNumAxes(Amotor,(int)axisnum); +Console.WriteLine("AxisNum={0}",(int)axisnum); + + int i = 0; + + if (m_rotateEnable.X == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); +Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); + i++; + } + + if (m_rotateEnable.Y == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); +Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); + i++; + } + + if (m_rotateEnable.Z == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); +Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); + i++; + } + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); + d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); + d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); + d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); + } // else none are locked + } // end Rotation Update + + + // VEHICLE processing ========================================== if (m_type != Vehicle.TYPE_NONE) { // get body attitude @@ -3420,7 +3518,7 @@ Console.WriteLine(" JointCreateFixed"); //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); - // Rotation Axis Disables: + /* // Rotation Axis Disables: if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) { if (m_angularEnable.X == 0) @@ -3430,7 +3528,7 @@ Console.WriteLine(" JointCreateFixed"); if (m_angularEnable.Z == 0) angObjectVel.Z = 0f; } - + */ angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation // Vertical attractor section @@ -3500,35 +3598,58 @@ Console.WriteLine(" JointCreateFixed"); } // end VEHICLES else { + // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - // NON-'VEHICLES' are dealt with here - /// Dynamics Angular Lock ======================================================================== - if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) - { - d.Vector3 avel2 = d.BodyGetAngularVel(Body); - if (m_angularEnable.X == 0) - avel2.X = 0; - if (m_angularEnable.Y == 0) - avel2.Y = 0; - if (m_angularEnable.Z == 0) - avel2.Z = 0; - d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); - } - - - /// Dynamics Buoyancy =============================================================================== + + /// Dynamics Buoyancy //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up // NB Prims in ODE are no subject to global gravity + // This should only affect gravity operations + float m_mass = CalculateMass(); + // calculate z-force due togravity on object. fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass - if (m_usePID) + if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. { -//if(frcount == 0) Console.WriteLine("PID " + m_primName); - // KF - this is for object MoveToTarget. - - //if (!d.BodyIsEnabled(Body)) + fz = 0; // llMoveToTarget ignores gravity. + // it also ignores mass of object, and any physical resting on it. + // Vector3 m_PIDTarget is where we are going + // float m_PIDTau is time to get there + fx = 0; + fy = 0; + d.Vector3 pos = d.BodyGetPosition(Body); + Vector3 error = new Vector3( + (m_PIDTarget.X - pos.X), + (m_PIDTarget.Y - pos.Y), + (m_PIDTarget.Z - pos.Z)); + if (error.ApproxEquals(Vector3.Zero,0.01f)) + { // Very close, Jump there and quit move + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + _target_velocity = Vector3.Zero; + d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + else + { + float scale = 50.0f * timestep / m_PIDTau; + if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero)) + { + // Nearby, quit update of velocity + } + else + { // Far, calc damped velocity + _target_velocity = error * scale; + } + d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + } // end PID MoveToTarget + + /* Original OS implementation: Does not work correctly as another phys object resting on THIS object purturbs its position. + This is incorrect behavior. llMoveToTarget must move the Body no matter what phys object is resting on it. + + //if (!d.BodyIsEnabled(Body)) //d.BodySetForce(Body, 0f, 0f, 0f); // no lock; for now it's only called from within Simulate() @@ -3559,6 +3680,7 @@ Console.WriteLine(" JointCreateFixed"); (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) ); +if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName, m_buoyancy, fz, _target_velocity); // if velocity is zero, use position control; otherwise, velocity control if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) @@ -3591,9 +3713,11 @@ Console.WriteLine(" JointCreateFixed"); fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); } } // end if (m_usePID) - + End of old PID system */ + + /// Dynamics Hover =================================================================================== - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + // Hover PID Controller can only run if the PIDcontroller is not in use. if (m_useHoverPID && !m_usePID) { //Console.WriteLine("Hover " + m_primName); @@ -3709,14 +3833,14 @@ Console.WriteLine(" JointCreateFixed"); // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); RLAservo = timestep / m_APIDStrength * scaler; rotforce = rotforce * RLAservo * diff_angle ; - + /* if (m_angularEnable.X == 0) rotforce.X = 0; if (m_angularEnable.Y == 0) rotforce.Y = 0; if (m_angularEnable.Z == 0) rotforce.Z = 0; - + */ d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); } @@ -3763,11 +3887,12 @@ Console.WriteLine(" JointCreateFixed"); fy = nmin; d.BodyAddForce(Body, fx, fy, fz); //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - } - } - else - { // is not physical, or is not a body or is selected + } // end apply forces + } // end Dynamics + +/* obsolete? + else + { // is not physical, or is not a body or is selected // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating _velocity.X = 0; _velocity.Y = 0; @@ -3781,8 +3906,11 @@ Console.WriteLine(" JointCreateFixed"); m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; _zeroFlag = true; - return; - } - } // end Move() + return; + } + */ + } // end root prims + + } // end Move() } // end class } -- cgit v1.1 From 7ab103c96f3e82cac592f20d61358e1928da8e27 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Tue, 13 Jul 2010 20:37:13 +0000 Subject: Rewrite collision dictionary handling, cleans up deleted obects/avs. Fixes occasional collision event failure. ChOde only. --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 90 +++++++++++++++++----- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 67 +++++++--------- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 6 ++ 3 files changed, 103 insertions(+), 60 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 38c38b6..61c16b8 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -565,7 +565,29 @@ namespace OpenSim.Region.Physics.OdePlugin CAPSULE_RADIUS = 0.01f; } + + if(Shell != IntPtr.Zero) + { + try + { + d.GeomDestroy(Shell); + } + catch (System.AccessViolationException) + { + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + // Remove any old entries +//string tShell; +//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell); +//Console.WriteLine("**** Remove {0}", tShell); + if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell); + if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell); + } + Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; +//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, m_name); d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); d.GeomSetCollideBits(Shell, (int)m_collisionFlags); @@ -931,10 +953,23 @@ namespace OpenSim.Region.Physics.OdePlugin Body = IntPtr.Zero; } - if (Shell != IntPtr.Zero) + if(Shell != IntPtr.Zero) { - d.GeomDestroy(Shell); - _parent_scene.geom_name_map.Remove(Shell); + try + { + d.GeomDestroy(Shell); + } + catch (System.AccessViolationException) + { + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + // Remove any old entries +//string tShell; +//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell); +//Console.WriteLine("**** Remove {0}", tShell); + + if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell); + if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell); Shell = IntPtr.Zero; } @@ -1097,11 +1132,24 @@ namespace OpenSim.Region.Physics.OdePlugin Body = IntPtr.Zero; } - - if (Shell != IntPtr.Zero) + + if(Shell != IntPtr.Zero) { - d.GeomDestroy(Shell); - _parent_scene.geom_name_map.Remove(Shell); + try + { + d.GeomDestroy(Shell); + } + catch (System.AccessViolationException) + { + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + // Remove any old entries +//string tShell; +//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell); +//Console.WriteLine("**** Remove {0}", tShell); + + if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell); + if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell); Shell = IntPtr.Zero; } } @@ -1277,9 +1325,6 @@ namespace OpenSim.Region.Physics.OdePlugin + (Amotor!=IntPtr.Zero ? "Amotor ":"")); } AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor); - - _parent_scene.geom_name_map[Shell] = m_name; - _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; _parent_scene.AddCharacter(this); } else @@ -1299,17 +1344,28 @@ namespace OpenSim.Region.Physics.OdePlugin { //kill the body d.BodyDestroy(Body); - Body = IntPtr.Zero; } - if (Shell != IntPtr.Zero) + if(Shell != IntPtr.Zero) { - d.GeomDestroy(Shell); - _parent_scene.geom_name_map.Remove(Shell); + try + { + d.GeomDestroy(Shell); + } + catch (System.AccessViolationException) + { + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + // Remove any old entries +//string tShell; +//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell); +//Console.WriteLine("**** Remove {0}", tShell); + + if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell); + if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell); Shell = IntPtr.Zero; } - } m_isPhysical = m_tainted_isPhysical; @@ -1327,13 +1383,9 @@ namespace OpenSim.Region.Physics.OdePlugin CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); d.BodyDestroy(Body); - d.GeomDestroy(Shell); AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor); Velocity = Vector3.Zero; - - _parent_scene.geom_name_map[Shell] = m_name; - _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; } else { diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 7ce01dc..6b17ce7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -142,7 +142,7 @@ namespace OpenSim.Region.Physics.OdePlugin private OdeScene _parent_scene; public IntPtr m_targetSpace = IntPtr.Zero; public IntPtr prim_geom; - public IntPtr prev_geom; +// public IntPtr prev_geom; public IntPtr _triMeshData; private IntPtr _linkJointGroup = IntPtr.Zero; @@ -274,7 +274,7 @@ namespace OpenSim.Region.Physics.OdePlugin prim_geom = IntPtr.Zero; - prev_geom = IntPtr.Zero; +// prev_geom = IntPtr.Zero; if (!pos.IsFinite()) { @@ -776,13 +776,26 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetGeom(IntPtr geom) { - prev_geom = prim_geom; + if(prim_geom != IntPtr.Zero) + { + // Remove any old entries +//string tPA; +//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); +//Console.WriteLine("**** Remove {0}", tPA); + if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); + if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); + d.GeomDestroy(prim_geom); + } + prim_geom = geom; //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); if (prim_geom != IntPtr.Zero) { + _parent_scene.geom_name_map[prim_geom] = this.m_primName; + _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); +//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); } if (childPrim) @@ -1774,17 +1787,17 @@ namespace OpenSim.Region.Physics.OdePlugin public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) { //Console.WriteLine("CreateGeom:"); - if (_mesh != null) + if (_mesh != null) // Special - make mesh { setMesh(_parent_scene, _mesh); } - else + else // not a mesh { - if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) // special profile?? { - if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) + if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) // Equi-size { - if (((_size.X / 2f) > 0f)) + if (((_size.X / 2f) > 0f)) // Has size { _parent_scene.waitForSpaceUnlock(m_targetSpace); try @@ -1815,7 +1828,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } } - else + else // not equi-size { _parent_scene.waitForSpaceUnlock(m_targetSpace); try @@ -1832,7 +1845,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - else + else // not special profile { _parent_scene.waitForSpaceUnlock(m_targetSpace); try @@ -1894,9 +1907,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - _parent_scene.geom_name_map[prim_geom] = this.m_primName; - _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; - changeSelectedStatus(timestep); m_taintadd = false; @@ -2045,22 +2055,7 @@ Console.WriteLine(" JointCreateFixed"); { if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) { - - - if (prim_geom != IntPtr.Zero) - { - try - { - d.GeomDestroy(prim_geom); - prim_geom = IntPtr.Zero; - _mesh = null; - } - catch (System.AccessViolationException) - { - prim_geom = IntPtr.Zero; - m_log.Error("[PHYSICS]: PrimGeom dead"); - } - } + _mesh = null; //Console.WriteLine("changePhysicsStatus for " + m_primName ); changeadd(2f); } @@ -2120,8 +2115,6 @@ Console.WriteLine(" JointCreateFixed"); _parent_scene.waitForSpaceUnlock(m_targetSpace); d.SpaceRemove(m_targetSpace, prim_geom); } - d.GeomDestroy(prim_geom); - prim_geom = IntPtr.Zero; // we don't need to do space calculation because the client sends a position update also. // Construction of new prim @@ -2223,16 +2216,8 @@ Console.WriteLine(" JointCreateFixed"); disableBody(); } } - try - { - d.GeomDestroy(prim_geom); - } - catch (System.AccessViolationException) - { - prim_geom = IntPtr.Zero; - m_log.Error("[PHYSICS]: PrimGeom dead"); - } - prim_geom = IntPtr.Zero; + + // we don't need to do space calculation because the client sends a position update also. if (_size.X <= 0) _size.X = 0.01f; if (_size.Y <= 0) _size.Y = 0.01f; diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 79e2986..ab084fd 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -2206,6 +2206,12 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prim.prim_geom != IntPtr.Zero) { + +//string tPA; +//geom_name_map.TryGetValue(prim.prim_geom, out tPA); +//Console.WriteLine("**** Remove {0}", tPA); + if(geom_name_map.ContainsKey(prim.prim_geom)) geom_name_map.Remove(prim.prim_geom); + if(actor_name_map.ContainsKey(prim.prim_geom)) actor_name_map.Remove(prim.prim_geom); d.GeomDestroy(prim.prim_geom); prim.prim_geom = IntPtr.Zero; } -- cgit v1.1 From 236c0f8e8b5f9ef86c8614b53e5f404754c9e8d9 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Fri, 16 Jul 2010 22:17:06 +0000 Subject: Fix llSetForecAndTorque([0]) stops linear motion. Mantis #187 --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 6b17ce7..7e70db9 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -1735,6 +1735,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical) { disableBodySoft(); + + if (Body != IntPtr.Zero) + { + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); + } } } else @@ -1756,6 +1763,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } +/* Uhhh - stop the motion if the object is _selected_!! if (m_isphysical) { if (Body != IntPtr.Zero) @@ -1765,6 +1773,7 @@ namespace OpenSim.Region.Physics.OdePlugin enableBodySoft(); } } +*/ } resetCollisionAccounting(); -- cgit v1.1 From 8f2986bc6a438ea9f5de912a5b76a196e7933175 Mon Sep 17 00:00:00 2001 From: Tom Date: Mon, 2 Aug 2010 21:54:47 -0700 Subject: Catch meshing error in Meshmerizer --- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index fded95e..fb2b462 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -336,16 +336,24 @@ namespace OpenSim.Region.Physics.Meshing bool mirror = ((primShape.SculptType & 128) != 0); bool invert = ((primShape.SculptType & 64) != 0); - sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, false, mirror, invert); - - idata.Dispose(); + try + { + sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, false, mirror, invert); + + idata.Dispose(); - sculptMesh.DumpRaw(baseDir, primName, "primMesh"); + sculptMesh.DumpRaw(baseDir, primName, "primMesh"); - sculptMesh.Scale(size.X, size.Y, size.Z); + sculptMesh.Scale(size.X, size.Y, size.Z); - coords = sculptMesh.coords; - faces = sculptMesh.faces; + coords = sculptMesh.coords; + faces = sculptMesh.faces; + } + catch(Exception ex) + { + m_log.Error("[Meshing]: Unable to generate a Sculpty mesh. " + ex.Message); + return null; + } } else { -- cgit v1.1 From d2d1c63b17a55ed1f324fafff89cfbcbd6618d11 Mon Sep 17 00:00:00 2001 From: Melanie Thielker Date: Tue, 3 Aug 2010 05:25:01 +0200 Subject: Revert "Catch meshing error in Meshmerizer" This reverts commit 8f2986bc6a438ea9f5de912a5b76a196e7933175. In favor of a fix that tells us where the issue is instead of eating the error --- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 22 +++++++--------------- 1 file changed, 7 insertions(+), 15 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index fb2b462..fded95e 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -336,24 +336,16 @@ namespace OpenSim.Region.Physics.Meshing bool mirror = ((primShape.SculptType & 128) != 0); bool invert = ((primShape.SculptType & 64) != 0); - try - { - sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, false, mirror, invert); - - idata.Dispose(); + sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, false, mirror, invert); + + idata.Dispose(); - sculptMesh.DumpRaw(baseDir, primName, "primMesh"); + sculptMesh.DumpRaw(baseDir, primName, "primMesh"); - sculptMesh.Scale(size.X, size.Y, size.Z); + sculptMesh.Scale(size.X, size.Y, size.Z); - coords = sculptMesh.coords; - faces = sculptMesh.faces; - } - catch(Exception ex) - { - m_log.Error("[Meshing]: Unable to generate a Sculpty mesh. " + ex.Message); - return null; - } + coords = sculptMesh.coords; + faces = sculptMesh.faces; } else { -- cgit v1.1 From 1d1da5069ca91282228f453476bee2444138b230 Mon Sep 17 00:00:00 2001 From: Tom Date: Tue, 3 Aug 2010 14:11:11 -0700 Subject: Catch a meshing exception caused by corrupt or missing sculpt maps, and mark the prim as "mesh failed" so it doesn't try meshing continuously --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 7e70db9..60eda41 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -131,6 +131,7 @@ namespace OpenSim.Region.Physics.OdePlugin //public GCHandle gc; private CollisionLocker ode; + private bool m_meshfailed = false; private bool m_taintforce = false; private bool m_taintaddangularforce = false; private Vector3 m_force; @@ -1882,12 +1883,20 @@ namespace OpenSim.Region.Physics.OdePlugin m_targetSpace = targetspace; - if (_mesh == null) + if (_mesh == null && m_meshfailed == false) { if (_parent_scene.needsMeshing(_pbs)) { // Don't need to re-enable body.. it's done in SetMesh - _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); + try + { + _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); + } + catch + { + //Don't continuously try to mesh prims when meshing has failed + m_meshfailed = true; + } // createmesh returns null when it's a shape that isn't a cube. // m_log.Debug(m_localID); } -- cgit v1.1 From e3cea888fba0afbb97e55b1a2aa8335f78a4ac26 Mon Sep 17 00:00:00 2001 From: Tom Date: Tue, 3 Aug 2010 16:07:18 -0700 Subject: Fix some more issues causing regions with corrupt sculpts to freeze. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 60eda41..1604c4b 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -2136,7 +2136,7 @@ Console.WriteLine(" JointCreateFixed"); // we don't need to do space calculation because the client sends a position update also. // Construction of new prim - if (_parent_scene.needsMeshing(_pbs)) + if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false) { float meshlod = _parent_scene.meshSculptLOD; @@ -2146,8 +2146,15 @@ Console.WriteLine(" JointCreateFixed"); IMesh mesh = null; - if (_parent_scene.needsMeshing(_pbs)) - mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + try + { + if (_parent_scene.needsMeshing(_pbs)) + mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + } + catch + { + m_meshfailed = true; + } //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); //Console.WriteLine("changesize 1"); @@ -2242,17 +2249,23 @@ Console.WriteLine(" JointCreateFixed"); if (_size.Z <= 0) _size.Z = 0.01f; // Construction of new prim - if (_parent_scene.needsMeshing(_pbs)) + if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false) { // Don't need to re-enable body.. it's done in SetMesh float meshlod = _parent_scene.meshSculptLOD; if (IsPhysical) meshlod = _parent_scene.MeshSculptphysicalLOD; - - IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + try + { + IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + CreateGeom(m_targetSpace, mesh); + } + catch + { + m_meshfailed = true; + } // createmesh returns null when it doesn't mesh. - CreateGeom(m_targetSpace, mesh); } else { -- cgit v1.1 From cb3b124df9213bf12383a5625ac4e545fcc91b87 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Wed, 4 Aug 2010 21:08:00 +0000 Subject: Fixes fresh hollowed prim collision shape. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 7e70db9..6aa28e0 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -1331,10 +1331,10 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - if (prim_geom == IntPtr.Zero) - { + // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer + // { SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); - } + // } } catch (AccessViolationException) { -- cgit v1.1 From 16814dd8f18b51f5dd8841ae341847e117bb83b3 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 2 Sep 2010 21:51:19 +0000 Subject: Fixes to JUMP system. --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 61c16b8..0c79aa6 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -90,6 +90,7 @@ namespace OpenSim.Region.Physics.OdePlugin public float walkDivisor = 1.3f; public float runDivisor = 0.8f; private bool flying = false; + private bool jumping = false; // add for jumping private bool m_iscolliding = false; private bool m_iscollidingGround = false; private bool m_wascolliding = false; @@ -229,7 +230,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool Selected { - set { return; } +// set { return; } + set { jumping = value; } // add for jumping flag } public override float Buoyancy @@ -1018,7 +1020,8 @@ namespace OpenSim.Region.Physics.OdePlugin { // close, jump to lateral destination d.BodySetPosition(Body, _zeroPosition.X, _zeroPosition.Y, pos.Z); } - if (flying) +// if (flying) + if (flying || jumping) // add for jumping { vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; } -- cgit v1.1 From c719e016ed9bfbb56fd8c571b9b7b9622bcef566 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Mon, 6 Sep 2010 21:45:07 +0000 Subject: Falling animation fix, comment out instrumentation. --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 21 +++++++++++++++------ OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 18 ++++++++++++++---- 2 files changed, 29 insertions(+), 10 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 0c79aa6..ae63cfa 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -269,7 +269,10 @@ namespace OpenSim.Region.Physics.OdePlugin /// public override bool IsColliding { - get { return m_iscolliding; } +//#@ get { return m_iscolliding; } + get { //## +//Console.WriteLine(">>>>>>>>>>>> IC get = {0}", m_iscolliding); //## + return m_iscolliding; } //## set { int i; @@ -307,6 +310,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_iscolliding = true; } +// ## Console.WriteLine("IC SET = {0} t{1} f{2} i {3}", value, truecount, falsecount, m_iscolliding); if (m_wascolliding != m_iscolliding) { //base.SendCollisionUpdate(new CollisionEventUpdate()); @@ -589,7 +593,7 @@ namespace OpenSim.Region.Physics.OdePlugin Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); _parent_scene.geom_name_map[Shell] = m_name; _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; -//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, m_name); +Console.WriteLine("**** Create {2} Dicts: actor={0} name={1} height={3} rad={4}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, m_name, CAPSULE_LENGTH, CAPSULE_RADIUS); d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); d.GeomSetCollideBits(Shell, (int)m_collisionFlags); @@ -857,7 +861,9 @@ namespace OpenSim.Region.Physics.OdePlugin { m_pidControllerActive = false; force *= 100f; - doForce(force); +//Console.WriteLine("DF 1"); // ## + if (!force.ApproxEquals(Vector3.Zero, 0.01f)) + doForce(force); // If uncommented, things get pushed off world // // m_log.Debug("Push!"); @@ -896,7 +902,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyAddForce(Body, force.X, force.Y, force.Z); //d.BodySetRotation(Body, ref m_StandUpRotation); //standupStraight(); - + d.Vector3 vel = d.BodyGetLinearVel(Body); //## +//Console.WriteLine("AvVel <{0},{1},{2}>", vel.X, vel.Y, vel.Z); //## } } @@ -1103,12 +1110,14 @@ namespace OpenSim.Region.Physics.OdePlugin } if (vec.IsFinite()) { - if (!vec.ApproxEquals(Vector3.Zero, 0.01f)) + if (!vec.ApproxEquals(Vector3.Zero, 0.02f)) // 0.01 allows 0.002 !! { +//Console.WriteLine("DF 2"); // ## + doForce(vec); if (!_zeroFlag) { - AlignAvatarTiltWithCurrentDirectionOfMovement(vec); +// AlignAvatarTiltWithCurrentDirectionOfMovement(vec); } } } diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index ab084fd..86f9893 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -713,7 +713,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Test if we're colliding a geom with a space. // If so we have to drill down into the space recursively - +//Console.WriteLine("near -----------"); //## if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) { if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) @@ -844,11 +844,21 @@ namespace OpenSim.Region.Physics.OdePlugin // Testing if the collision is at the feet of the avatar //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); - if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)) - p2.IsColliding = true; - } +//#@ if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)) +//#@ p2.IsColliding = true; + if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)){ //## +//Console.WriteLine("AvColl 1 {0} - {1} - {2} - {3}", //## +// curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); //## + p2.IsColliding = true; //## + }else{ +//Console.WriteLine("AvColl 2 {0} - {1} - {2} - {3}", //## +// curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); //## + + } //## + } else { +//Console.WriteLine("AvColl 3 {0}", p2.PhysicsActorType); //## p2.IsColliding = true; } -- cgit v1.1 From 83b16612ce13ee89f139df8fdc78ded14cb6a69e Mon Sep 17 00:00:00 2001 From: Melanie Date: Sat, 25 Sep 2010 05:57:08 +0200 Subject: Experimental locking of taint processing --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 156 +++++++++++---------- 1 file changed, 80 insertions(+), 76 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index ae63cfa..505d455 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -143,6 +143,7 @@ namespace OpenSim.Region.Physics.OdePlugin // unique UUID of this character object public UUID m_uuid; public bool bad = false; + private Object m_syncRoot = new Object(); public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor) { @@ -1323,103 +1324,106 @@ Console.WriteLine("**** Create {2} Dicts: actor={0} name={1} height={3} ra public void ProcessTaints(float timestep) { - - if (m_tainted_isPhysical != m_isPhysical) + lock (m_syncRoot) { - if (m_tainted_isPhysical) - { - // Create avatar capsule and related ODE data - if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero)) - { - m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " - + (Shell!=IntPtr.Zero ? "Shell ":"") - + (Body!=IntPtr.Zero ? "Body ":"") - + (Amotor!=IntPtr.Zero ? "Amotor ":"")); - } - AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor); - _parent_scene.AddCharacter(this); - } - else - { - _parent_scene.RemoveCharacter(this); - // destroy avatar capsule and related ODE data - if (Amotor != IntPtr.Zero) - { - // Kill the Amotor - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - //kill the Geometry - _parent_scene.waitForSpaceUnlock(_parent_scene.space); - if (Body != IntPtr.Zero) + if (m_tainted_isPhysical != m_isPhysical) + { + if (m_tainted_isPhysical) { - //kill the body - d.BodyDestroy(Body); - Body = IntPtr.Zero; + // Create avatar capsule and related ODE data + if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero)) + { + m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " + + (Shell!=IntPtr.Zero ? "Shell ":"") + + (Body!=IntPtr.Zero ? "Body ":"") + + (Amotor!=IntPtr.Zero ? "Amotor ":"")); + } + AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor); + _parent_scene.AddCharacter(this); } - - if(Shell != IntPtr.Zero) + else { - try + _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + if (Amotor != IntPtr.Zero) { - d.GeomDestroy(Shell); + // Kill the Amotor + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; } - catch (System.AccessViolationException) + //kill the Geometry + _parent_scene.waitForSpaceUnlock(_parent_scene.space); + + if (Body != IntPtr.Zero) { - m_log.Error("[PHYSICS]: PrimGeom dead"); + //kill the body + d.BodyDestroy(Body); + Body = IntPtr.Zero; } - // Remove any old entries -//string tShell; -//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell); -//Console.WriteLine("**** Remove {0}", tShell); - if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell); - if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell); - Shell = IntPtr.Zero; + if(Shell != IntPtr.Zero) + { + try + { + d.GeomDestroy(Shell); + } + catch (System.AccessViolationException) + { + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + // Remove any old entries + //string tShell; + //_parent_scene.geom_name_map.TryGetValue(Shell, out tShell); + //Console.WriteLine("**** Remove {0}", tShell); + + if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell); + if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } } - } - m_isPhysical = m_tainted_isPhysical; - } + m_isPhysical = m_tainted_isPhysical; + } - if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) - { - if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) { + if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + { - m_pidControllerActive = true; - // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate() - d.JointDestroy(Amotor); - float prevCapsule = CAPSULE_LENGTH; - CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; - //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); - d.BodyDestroy(Body); - AvatarGeomAndBodyCreation(_position.X, _position.Y, - _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor); - Velocity = Vector3.Zero; - } - else - { - m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " - + (Shell==IntPtr.Zero ? "Shell ":"") - + (Body==IntPtr.Zero ? "Body ":"") - + (Amotor==IntPtr.Zero ? "Amotor ":"")); + m_pidControllerActive = true; + // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate() + d.JointDestroy(Amotor); + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + d.BodyDestroy(Body); + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor); + Velocity = Vector3.Zero; + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell==IntPtr.Zero ? "Shell ":"") + + (Body==IntPtr.Zero ? "Body ":"") + + (Amotor==IntPtr.Zero ? "Amotor ":"")); + } } - } - if (!m_taintPosition.ApproxEquals(_position, 0.05f)) - { - if (Body != IntPtr.Zero) + if (!m_taintPosition.ApproxEquals(_position, 0.05f)) { - d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); + if (Body != IntPtr.Zero) + { + d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); - _position.X = m_taintPosition.X; - _position.Y = m_taintPosition.Y; - _position.Z = m_taintPosition.Z; + _position.X = m_taintPosition.X; + _position.Y = m_taintPosition.Y; + _position.Z = m_taintPosition.Z; + } } - } + } } internal void AddCollisionFrameTime(int p) -- cgit v1.1 From 6f689f591a295195cf8db3c37bcd382502d468d7 Mon Sep 17 00:00:00 2001 From: Melanie Date: Sun, 26 Sep 2010 17:56:31 +0100 Subject: Merge Master --- OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs | 4 ++-- OpenSim/Region/Physics/Manager/ZeroMesher.cs | 3 ++- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 13 +++++++++---- OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs | 6 +++++- 4 files changed, 18 insertions(+), 8 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs b/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs index 7130a3e..3763696 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs @@ -81,7 +81,7 @@ namespace OpenSim.Region.Physics.Manager if (_MeshPlugins.ContainsKey(meshEngineName)) { m_log.Info("[PHYSICS]: creating meshing engine " + meshEngineName); - meshEngine = _MeshPlugins[meshEngineName].GetMesher(); + meshEngine = _MeshPlugins[meshEngineName].GetMesher(config); } else { @@ -234,6 +234,6 @@ namespace OpenSim.Region.Physics.Manager public interface IMeshingPlugin { string GetName(); - IMesher GetMesher(); + IMesher GetMesher(IConfigSource config); } } diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs index e6e75f9..ba19db6 100644 --- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs +++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs @@ -28,6 +28,7 @@ using System; using OpenSim.Framework; using OpenMetaverse; +using Nini.Config; /* * This is the zero mesher. @@ -53,7 +54,7 @@ namespace OpenSim.Region.Physics.Manager return "ZeroMesher"; } - public IMesher GetMesher() + public IMesher GetMesher(IConfigSource config) { return new ZeroMesher(); } diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index fded95e..62f947c 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -35,6 +35,7 @@ using System.Drawing; using System.Drawing.Imaging; using PrimMesher; using log4net; +using Nini.Config; using System.Reflection; using System.IO; @@ -51,9 +52,9 @@ namespace OpenSim.Region.Physics.Meshing return "Meshmerizer"; } - public IMesher GetMesher() + public IMesher GetMesher(IConfigSource config) { - return new Meshmerizer(); + return new Meshmerizer(config); } } @@ -70,14 +71,18 @@ namespace OpenSim.Region.Physics.Meshing #endif private bool cacheSculptMaps = true; - private string decodedScultMapPath = "j2kDecodeCache"; + private string decodedScultMapPath = null; private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh private Dictionary m_uniqueMeshes = new Dictionary(); - public Meshmerizer() + public Meshmerizer(IConfigSource config) { + IConfig start_config = config.Configs["Startup"]; + + decodedScultMapPath = start_config.GetString("DecodedSculpMapPath","j2kDecodeCache"); + try { if (!Directory.Exists(decodedScultMapPath)) diff --git a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs index 69e2d03..ab8f8bf 100644 --- a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs +++ b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs @@ -48,6 +48,10 @@ namespace OpenSim.Region.Physics.OdePlugin [SetUp] public void Initialize() { + IConfigSource TopConfig = new IniConfigSource(); + IConfig config = TopConfig.AddConfig("Startup"); + config.Set("DecodedSculpMapPath","j2kDecodeCache"); + // Loading ODEPlugin cbt = new OdePlugin(); // Loading Zero Mesher @@ -55,7 +59,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Getting Physics Scene ps = cbt.GetScene("test"); // Initializing Physics Scene. - ps.Initialise(imp.GetMesher(),null); + ps.Initialise(imp.GetMesher(TopConfig),null); float[] _heightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize]; for (int i = 0; i < ((int)Constants.RegionSize * (int)Constants.RegionSize); i++) { -- cgit v1.1 From aecac9bba9b89682f53ebde1e182ad04f5b2dc99 Mon Sep 17 00:00:00 2001 From: Melanie Date: Sun, 26 Sep 2010 18:05:55 +0100 Subject: Typo fixes --- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 12 ++++++------ OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index 62f947c..20a5bb4 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -71,7 +71,7 @@ namespace OpenSim.Region.Physics.Meshing #endif private bool cacheSculptMaps = true; - private string decodedScultMapPath = null; + private string decodedSculptMapPath = null; private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh @@ -81,16 +81,16 @@ namespace OpenSim.Region.Physics.Meshing { IConfig start_config = config.Configs["Startup"]; - decodedScultMapPath = start_config.GetString("DecodedSculpMapPath","j2kDecodeCache"); + decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); try { - if (!Directory.Exists(decodedScultMapPath)) - Directory.CreateDirectory(decodedScultMapPath); + if (!Directory.Exists(decodedSculptMapPath)) + Directory.CreateDirectory(decodedSculptMapPath); } catch (Exception e) { - m_log.WarnFormat("[SCULPT]: Unable to create {0} directory: ", decodedScultMapPath, e.Message); + m_log.WarnFormat("[SCULPT]: Unable to create {0} directory: ", decodedSculptMapPath, e.Message); } } @@ -265,7 +265,7 @@ namespace OpenSim.Region.Physics.Meshing { if (cacheSculptMaps && primShape.SculptTexture != UUID.Zero) { - decodedSculptFileName = System.IO.Path.Combine(decodedScultMapPath, "smap_" + primShape.SculptTexture.ToString()); + decodedSculptFileName = System.IO.Path.Combine(decodedSculptMapPath, "smap_" + primShape.SculptTexture.ToString()); try { if (File.Exists(decodedSculptFileName)) diff --git a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs index ab8f8bf..a7f8baa 100644 --- a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs +++ b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs @@ -50,7 +50,7 @@ namespace OpenSim.Region.Physics.OdePlugin { IConfigSource TopConfig = new IniConfigSource(); IConfig config = TopConfig.AddConfig("Startup"); - config.Set("DecodedSculpMapPath","j2kDecodeCache"); + config.Set("DecodedSculptMapPath","j2kDecodeCache"); // Loading ODEPlugin cbt = new OdePlugin(); -- cgit v1.1 From 9f7f266f583cfd47f3c66aa56b0ba26e634e8d91 Mon Sep 17 00:00:00 2001 From: Melanie Date: Sat, 2 Oct 2010 00:18:52 +0200 Subject: Replace CalculateMass with a better, contributed version --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 471 +++++++++++++------------- 1 file changed, 228 insertions(+), 243 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 2105be1..793774a 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -884,300 +884,285 @@ namespace OpenSim.Region.Physics.OdePlugin private float CalculateMass() { - float volume = 0; - - // No material is passed to the physics engines yet.. soo.. - // we're using the m_density constant in the class definition + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; float returnMass = 0; - + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + switch (_pbs.ProfileShape) { case ProfileShape.Square: - // Profile Volume + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; - volume = _size.X*_size.Y*_size.Z; + case HollowShape.Circle: - // If the user has 'hollowed out' - // ProfileHollow is one of those 0 to 50000 values :P - // we like percentages better.. so turning into a percentage + hollowVolume *= 0.78539816339f; + break; - if (((float) _pbs.ProfileHollow/50000f) > 0.0) - { - float hollowAmount = (float) _pbs.ProfileHollow/50000f; + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } - // calculate the hollow volume by it's shape compared to the prim shape - float hollowVolume = 0; - switch (_pbs.HollowShape) + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - case HollowShape.Square: - case HollowShape.Same: - // Cube Hollow volume calculation - float hollowsizex = _size.X*hollowAmount; - float hollowsizey = _size.Y*hollowAmount; - float hollowsizez = _size.Z*hollowAmount; - hollowVolume = hollowsizex*hollowsizey*hollowsizez; - break; - - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - float hRadius = _size.X/2; - float hLength = _size.Z; - - // pi * r2 * h - hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount); - break; - - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y - - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount); - break; - - default: - hollowVolume = 0; - break; + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume*tmp*tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f;; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } } - volume = volume - hollowVolume; - } break; + case ProfileShape.Circle: - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - // Cylinder - float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z); - float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z); - // Approximating the cylinder's irregularity. - if (volume1 > volume2) - { - volume = (float)volume1 - (volume1 - volume2); - } - else if (volume2 > volume1) + if (_pbs.PathCurve == (byte)Extrusion.Straight) { - volume = (float)volume2 - (volume2 - volume1); + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } } - else + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - // Regular cylinder - volume = volume1; - } - } - else - { - // We don't know what the shape is yet, so use default - volume = _size.X * _size.Y * _size.Z; - } - // If the user has 'hollowed out' - // ProfileHollow is one of those 0 to 50000 values :P - // we like percentages better.. so turning into a percentage + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { - if (((float)_pbs.ProfileHollow / 50000f) > 0.0) - { - float hollowAmount = (float)_pbs.ProfileHollow / 50000f; + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; - // calculate the hollow volume by it's shape compared to the prim shape - float hollowVolume = 0; - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - float hRadius = _size.X / 2; - float hLength = _size.Z; - - // pi * r2 * h - hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount); - break; - - case HollowShape.Square: - // Cube Hollow volume calculation - float hollowsizex = _size.X * hollowAmount; - float hollowsizey = _size.Y * hollowAmount; - float hollowsizez = _size.Z * hollowAmount; - hollowVolume = hollowsizex * hollowsizey * hollowsizez; - break; - - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y - - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); - break; - - default: - hollowVolume = 0; - break; + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } } - volume = volume - hollowVolume; - } break; case ProfileShape.HalfCircle: if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - if (_size.X == _size.Y && _size.Y == _size.Z) - { - // regular sphere - // v = 4/3 * pi * r^3 - float sradius3 = (float)Math.Pow((_size.X / 2), 3); - volume = (float)((4f / 3f) * Math.PI * sradius3); - } - else - { - // we treat this as a box currently - volume = _size.X * _size.Y * _size.Z; - } - } - else - { - // We don't know what the shape is yet, so use default - volume = _size.X * _size.Y * _size.Z; + volume *= 0.52359877559829887307710723054658f; } break; case ProfileShape.EquilateralTriangle: - /* - v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h - // seed mesh - Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f); - Vertex PM = new Vertex(+0.5f, 0f, 0.0f); - Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f); - */ - float xA = -0.25f * _size.X; - float yA = -0.45f * _size.Y; + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; - float xB = 0.5f * _size.X; - float yB = 0; + if (hollowAmount > 0.0) + { - float xC = -0.25f * _size.X; - float yC = 0.45f * _size.Y; + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; - volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z); + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; - // If the user has 'hollowed out' - // ProfileHollow is one of those 0 to 50000 values :P - // we like percentages better.. so turning into a percentage - float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f); - if (((float)fhollowFactor / 50000f) > 0.0) - { - float hollowAmount = (float)fhollowFactor / 50000f; + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation - // calculate the hollow volume by it's shape compared to the prim shape - float hollowVolume = 0; - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y - - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); - break; - - case HollowShape.Square: - // Cube Hollow volume calculation - float hollowsizex = _size.X * hollowAmount; - float hollowsizey = _size.Y * hollowAmount; - float hollowsizez = _size.Z * hollowAmount; - hollowVolume = hollowsizex * hollowsizey * hollowsizez; - break; - - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - float hRadius = _size.X / 2; - float hLength = _size.Z; - - // pi * r2 * h - hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount); - break; - - default: - hollowVolume = 0; - break; + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } } - volume = volume - hollowVolume; - } - break; + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); - default: - // we don't have all of the volume formulas yet so - // use the common volume formula for all - volume = _size.X*_size.Y*_size.Z; - break; - } + if (hollowAmount > 0.0) + { - // Calculate Path cut effect on volume - // Not exact, in the triangle hollow example - // They should never be zero or less then zero.. - // we'll ignore it if it's less then zero + hollowVolume *= hollowAmount; - // ProfileEnd and ProfileBegin are values - // from 0 to 50000 + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; - // Turning them back into percentages so that I can cut that percentage off the volume + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; - float PathCutEndAmount = _pbs.ProfileEnd; - float PathCutStartAmount = _pbs.ProfileBegin; - if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f) - { - float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f); + case HollowShape.Circle: - // Check the return amount for sanity - if (pathCutAmount >= 0.99f) - pathCutAmount = 0.99f; + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; - volume = volume - (volume*pathCutAmount); - } - UInt16 taperX = _pbs.PathScaleX; - UInt16 taperY = _pbs.PathScaleY; - float taperFactorX = 0; - float taperFactorY = 0; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; - // Mass = density * volume - if (taperX != 100) - { - if (taperX > 100) - { - taperFactorX = 1.0f - ((float)taperX / 200); - //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString()); - } - else - { - taperFactorX = 1.0f - ((100 - (float)taperX) / 100); - //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString()); + default: + break; } - volume = (float)volume * ((taperFactorX / 3f) + 0.001f); - } - if (taperY != 100) - { - if (taperY > 100) + + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) { - taperFactorY = 1.0f - ((float)taperY / 200); - //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString()); + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; } - else + else { - taperFactorY = 1.0f - ((100 - (float)taperY) / 100); - //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString()); + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } - volume = (float)volume * ((taperFactorY / 3f) + 0.001f); - } - returnMass = m_density*volume; - if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. + + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + +// this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + returnMass = m_density * volume; + + if (returnMass <= 0) + returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. +// else if (returnMass > _parent_scene.maximumMassObject) +// returnMass = _parent_scene.maximumMassObject; + -- cgit v1.1 From b40c91777cf272b485da0cdfba2ffec868cfc7cf Mon Sep 17 00:00:00 2001 From: Melanie Date: Sat, 2 Oct 2010 00:19:30 +0200 Subject: Replace CalculateMass with a more accurate version, contributed by Ubit. Thank you. --- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 453 ++++++++++++++-------------- 1 file changed, 219 insertions(+), 234 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index daf7fb0..319a951 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -403,300 +403,285 @@ namespace OpenSim.Region.Physics.OdePlugin private float CalculateMass() { - float volume = 0; - - // No material is passed to the physics engines yet.. soo.. - // we're using the m_density constant in the class definition + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; float returnMass = 0; - + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + switch (_pbs.ProfileShape) { case ProfileShape.Square: - // Profile Volume + // default box - volume = _size.X*_size.Y*_size.Z; + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; - // If the user has 'hollowed out' - // ProfileHollow is one of those 0 to 50000 values :P - // we like percentages better.. so turning into a percentage + case HollowShape.Circle: - if (((float) _pbs.ProfileHollow/50000f) > 0.0) - { - float hollowAmount = (float) _pbs.ProfileHollow/50000f; + hollowVolume *= 0.78539816339f; + break; - // calculate the hollow volume by it's shape compared to the prim shape - float hollowVolume = 0; - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - // Cube Hollow volume calculation - float hollowsizex = _size.X*hollowAmount; - float hollowsizey = _size.Y*hollowAmount; - float hollowsizez = _size.Z*hollowAmount; - hollowVolume = hollowsizex*hollowsizey*hollowsizez; - break; + case HollowShape.Triangle: - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - float hRadius = _size.X/2; - float hLength = _size.Z; - - // pi * r2 * h - hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount); - break; + hollowVolume *= (0.5f * .5f); + break; - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount); - break; + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube - default: - hollowVolume = 0; - break; + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume*tmp*tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f;; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } } - volume = volume - hollowVolume; - } break; + case ProfileShape.Circle: - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - // Cylinder - float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z); - float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z); - // Approximating the cylinder's irregularity. - if (volume1 > volume2) - { - volume = (float)volume1 - (volume1 - volume2); - } - else if (volume2 > volume1) - { - volume = (float)volume2 - (volume2 - volume1); - } - else + if (_pbs.PathCurve == (byte)Extrusion.Straight) { - // Regular cylinder - volume = volume1; - } - } - else - { - // We don't know what the shape is yet, so use default - volume = _size.X * _size.Y * _size.Z; - } - // If the user has 'hollowed out' - // ProfileHollow is one of those 0 to 50000 values :P - // we like percentages better.. so turning into a percentage + volume *= 0.78539816339f; // elipse base - if (((float)_pbs.ProfileHollow / 50000f) > 0.0) - { - float hollowAmount = (float)_pbs.ProfileHollow / 50000f; + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; - // calculate the hollow volume by it's shape compared to the prim shape - float hollowVolume = 0; - switch (_pbs.HollowShape) + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - case HollowShape.Same: - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - float hRadius = _size.X / 2; - float hLength = _size.Z; - - // pi * r2 * h - hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount); - break; + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { - case HollowShape.Square: - // Cube Hollow volume calculation - float hollowsizex = _size.X * hollowAmount; - float hollowsizey = _size.Y * hollowAmount; - float hollowsizez = _size.Z * hollowAmount; - hollowVolume = hollowsizex * hollowsizey * hollowsizez; - break; + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); - break; + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; - default: - hollowVolume = 0; - break; + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } } - volume = volume - hollowVolume; - } break; case ProfileShape.HalfCircle: if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - if (_size.X == _size.Y && _size.Y == _size.Z) - { - // regular sphere - // v = 4/3 * pi * r^3 - float sradius3 = (float)Math.Pow((_size.X / 2), 3); - volume = (float)((4f / 3f) * Math.PI * sradius3); - } - else - { - // we treat this as a box currently - volume = _size.X * _size.Y * _size.Z; - } - } - else - { - // We don't know what the shape is yet, so use default - volume = _size.X * _size.Y * _size.Z; + volume *= 0.52359877559829887307710723054658f; } break; case ProfileShape.EquilateralTriangle: - /* - v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h - // seed mesh - Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f); - Vertex PM = new Vertex(+0.5f, 0f, 0.0f); - Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f); - */ - float xA = -0.25f * _size.X; - float yA = -0.45f * _size.Y; + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; - float xB = 0.5f * _size.X; - float yB = 0; + if (hollowAmount > 0.0) + { - float xC = -0.25f * _size.X; - float yC = 0.45f * _size.Y; + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; - volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z); + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; - // If the user has 'hollowed out' - // ProfileHollow is one of those 0 to 50000 values :P - // we like percentages better.. so turning into a percentage - float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f); - if (((float)fhollowFactor / 50000f) > 0.0) - { - float hollowAmount = (float)fhollowFactor / 50000f; + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation - // calculate the hollow volume by it's shape compared to the prim shape - float hollowVolume = 0; - switch (_pbs.HollowShape) + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - case HollowShape.Same: - case HollowShape.Triangle: - // Equilateral Triangular Prism volume hollow calculation - // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y - - float aLength = _size.Y; - // 1/2 abh - hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); - break; + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); - case HollowShape.Square: - // Cube Hollow volume calculation - float hollowsizex = _size.X * hollowAmount; - float hollowsizey = _size.Y * hollowAmount; - float hollowsizez = _size.Z * hollowAmount; - hollowVolume = hollowsizex * hollowsizey * hollowsizez; - break; + if (hollowAmount > 0.0) + { - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - float hRadius = _size.X / 2; - float hLength = _size.Z; + hollowVolume *= hollowAmount; - // pi * r2 * h - hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount); - break; + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; - default: - hollowVolume = 0; - break; - } - volume = volume - hollowVolume; - } - break; + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; - default: - // we don't have all of the volume formulas yet so - // use the common volume formula for all - volume = _size.X*_size.Y*_size.Z; - break; - } + case HollowShape.Circle: - // Calculate Path cut effect on volume - // Not exact, in the triangle hollow example - // They should never be zero or less then zero.. - // we'll ignore it if it's less then zero + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; - // ProfileEnd and ProfileBegin are values - // from 0 to 50000 + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; - // Turning them back into percentages so that I can cut that percentage off the volume + default: + break; + } - float PathCutEndAmount = _pbs.ProfileEnd; - float PathCutStartAmount = _pbs.ProfileBegin; - if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f) - { - float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f); - // Check the return amount for sanity - if (pathCutAmount >= 0.99f) - pathCutAmount = 0.99f; - volume = volume - (volume*pathCutAmount); - } - UInt16 taperX = _pbs.PathScaleX; - UInt16 taperY = _pbs.PathScaleY; - float taperFactorX = 0; - float taperFactorY = 0; + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; - // Mass = density * volume - if (taperX != 100) - { - if (taperX > 100) + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) { - taperFactorX = 1.0f - ((float)taperX / 200); - //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString()); - } - else - { - taperFactorX = 1.0f - ((100 - (float)taperX) / 100); - //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString()); - } - volume = (float)volume * ((taperFactorX / 3f) + 0.001f); - } + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; - if (taperY != 100) - { - if (taperY > 100) - { - taperFactorY = 1.0f - ((float)taperY / 200); - //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString()); + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; } - else + else { - taperFactorY = 1.0f - ((100 - (float)taperY) / 100); - //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString()); + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } - volume = (float)volume * ((taperFactorY / 3f) + 0.001f); - } - returnMass = m_density*volume; - if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. + + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + +// this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + returnMass = m_density * volume; + + if (returnMass <= 0) + returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. +// else if (returnMass > _parent_scene.maximumMassObject) +// returnMass = _parent_scene.maximumMassObject; + -- cgit v1.1 From bf695cccbabcc4c18ca26fac17922a1b79d25c33 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Fri, 3 Dec 2010 18:32:58 +0000 Subject: Fix spurious Av move when clothing item worn or removed. --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 505d455..93f9964 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -469,7 +469,7 @@ namespace OpenSim.Region.Physics.OdePlugin //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); Velocity = Vector3.Zero; - + m_taintPosition = _position; // update the stale taint position _parent_scene.AddPhysicsActorTaint(this); } else -- cgit v1.1 From 95a915efd88d859c3fc29576e599c6912522e6dc Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Sat, 11 Dec 2010 21:04:03 +0000 Subject: Fix border fence for physicals. Fix llRotLookAt() for Vehicles. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 210 +++++++----------------- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 4 +- 2 files changed, 61 insertions(+), 153 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 793774a..16e90ff 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -3001,12 +3001,9 @@ Console.WriteLine(" JointCreateFixed"); // This is a temp patch until proper region crossing is developed. int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; - int fence = _parent_scene.geomRegionFence; + float fence = _parent_scene.geomRegionFence; - float border_limit = 0.05f; // original limit - if (fence == 1) border_limit = 0.5f; // bounce point - - frcount++; // used to limit debug comment output + frcount++; // used to limit debug comment output if (frcount > 50) frcount = 0; @@ -3049,35 +3046,34 @@ Console.WriteLine(" JointCreateFixed"); // Check if outside region // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! - if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit)) + if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) { - l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit); + l_position.X = ((float)_parent_scene.WorldExtents.X - fence); outside = 1; } - if (l_position.X < border_limit) + if (l_position.X < fence) { - l_position.X = border_limit; + l_position.X = fence; outside = 2; } - if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit)) + if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence)) { - l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit); + l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence); outside = 3; } - if (l_position.Y < border_limit) + if (l_position.Y < fence) { - l_position.Y = border_limit; + l_position.Y = fence; outside = 4; } if (outside > 0) { -//Console.WriteLine(" fence = {0}",fence); -//Console.WriteLine("Border {0}", l_position); - if (fence == 1) // bounce object off boundary +//Console.WriteLine("Border {0} fence={1}", l_position, fence); + if (fence > 0.0f) // bounce object off boundary { if (revcount == 0) { @@ -3647,75 +3643,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); } } // end PID MoveToTarget - /* Original OS implementation: Does not work correctly as another phys object resting on THIS object purturbs its position. - This is incorrect behavior. llMoveToTarget must move the Body no matter what phys object is resting on it. - - //if (!d.BodyIsEnabled(Body)) - //d.BodySetForce(Body, 0f, 0f, 0f); - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - //PidStatus = true; - - // PhysicsVector vec = new PhysicsVector(); -// d.Vector3 vel = d.BodyGetLinearVel(Body); - - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); - -if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName, m_buoyancy, fz, _target_velocity); - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - // return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end if (m_usePID) - End of old PID system */ - /// Dynamics Hover =================================================================================== // Hover PID Controller can only run if the PIDcontroller is not in use. @@ -3802,51 +3729,6 @@ if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName } } // end m_useHoverPID && !m_usePID - /// Dynamics RotLookAt ================================================================================= - if (m_useAPID) - { - // RotLookAt, apparently overrides all other rotation sources. Inputs: - // Quaternion m_APIDTarget - // float m_APIDStrength // From SL experiments, this is the time to get there - // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly - // Also in SL the mass of the object has no effect on time to get there. - // Factors: - // get present body rotation - float limit = 1.0f; - float scaler = 50f; // adjusts damping time - float RLAservo = 0f; - - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; - float diff_angle; - Vector3 diff_axis; - rot_diff.GetAxisAngle(out diff_axis, out diff_angle); - diff_axis.Normalize(); - if(diff_angle > 0.01f) // diff_angle is always +ve - { -// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); - Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); - rotforce = rotforce * rotq; - if(diff_angle > limit) diff_angle = limit; // cap the rotate rate -// RLAservo = timestep / m_APIDStrength * m_mass * scaler; - // rotforce = rotforce * RLAservo * diff_angle ; - // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); - RLAservo = timestep / m_APIDStrength * scaler; - rotforce = rotforce * RLAservo * diff_angle ; - /* - if (m_angularEnable.X == 0) - rotforce.X = 0; - if (m_angularEnable.Y == 0) - rotforce.Y = 0; - if (m_angularEnable.Z == 0) - rotforce.Z = 0; - */ - d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); -//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); - } -//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); - } // end m_useAPID /// Dynamics Apply Forces =================================================================================== fx *= m_mass; @@ -3889,29 +3771,55 @@ if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName d.BodyAddForce(Body, fx, fy, fz); //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); } // end apply forces - } // end Dynamics + } // end Vehicle/Dynamics + + /// RotLookAt ================================================================================= + if (m_useAPID) + { + // RotLookAt, apparently overrides all other rotation sources. Inputs: + // Quaternion m_APIDTarget + // float m_APIDStrength // From SL experiments, this is the time to get there + // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly + // Also in SL the mass of the object has no effect on time to get there. + // Factors: + // get present body rotation + float limit = 1.0f; + float scaler = 50f; // adjusts damping time + float RLAservo = 0f; + + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; + float diff_angle; + Vector3 diff_axis; + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); + diff_axis.Normalize(); + if(diff_angle > 0.01f) // diff_angle is always +ve + { +// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if(diff_angle > limit) diff_angle = limit; // cap the rotate rate +// RLAservo = timestep / m_APIDStrength * m_mass * scaler; + // rotforce = rotforce * RLAservo * diff_angle ; + // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); + RLAservo = timestep / m_APIDStrength * scaler; + rotforce = rotforce * RLAservo * diff_angle ; + /* + if (m_angularEnable.X == 0) + rotforce.X = 0; + if (m_angularEnable.Y == 0) + rotforce.Y = 0; + if (m_angularEnable.Z == 0) + rotforce.Z = 0; + */ + d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); +//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } +//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); + } // end m_useAPID -/* obsolete? - else - { // is not physical, or is not a body or is selected - // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; - return; - } - */ } // end root prims - } // end Move() } // end class } diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 86f9893..e1989e2 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -224,7 +224,7 @@ namespace OpenSim.Region.Physics.OdePlugin public float bodyPIDG = 25; public int geomCrossingFailuresBeforeOutofbounds = 5; - public int geomRegionFence = 0; + public float geomRegionFence = 0.0f; public float bodyMotorJointMaxforceTensor = 2; @@ -448,7 +448,7 @@ namespace OpenSim.Region.Physics.OdePlugin geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); - geomRegionFence = physicsconfig.GetInt("region_border_fence", 0); + geomRegionFence = physicsconfig.GetFloat("region_border_fence", 0.0f); geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); -- cgit v1.1 From 5fccbe21d68f681b9a5aa0f2ae535a7d61a9a3f4 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Mon, 20 Dec 2010 07:04:35 +0000 Subject: Update materials parameters. --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 200 +++++++++++++----------- 1 file changed, 108 insertions(+), 92 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index e1989e2..756c005 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -491,43 +491,55 @@ namespace OpenSim.Region.Physics.OdePlugin staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)]; - // Centeral contact friction and bounce - // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why - // an avatar falls through in Z but not in X or Y when walking on a prim. - contact.surface.mode |= d.ContactFlags.SoftERP; - contact.surface.mu = nmAvatarObjectContactFriction; - contact.surface.bounce = nmAvatarObjectContactBounce; - contact.surface.soft_cfm = 0.010f; - contact.surface.soft_erp = 0.010f; + // Centeral contact friction and bounce // KF: This appears to be only for static AV on non-phys prim. + contact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + contact.surface.mu = 255.0f; + contact.surface.bounce = 0.0f; + contact.surface.soft_cfm = 0.0f; + contact.surface.soft_erp = 1.00f; // If this is too small static Av will fall through a sloping prim. // Terrain contact friction and Bounce // This is the *non* moving version. Use this when an avatar // isn't moving to keep it in place better - TerrainContact.surface.mode |= d.ContactFlags.SoftERP; +/* TerrainContact.surface.mode |= d.ContactFlags.SoftERP; TerrainContact.surface.mu = nmTerrainContactFriction; TerrainContact.surface.bounce = nmTerrainContactBounce; - TerrainContact.surface.soft_erp = nmTerrainContactERP; + TerrainContact.surface.soft_erp = nmTerrainContactERP; */ + + TerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + TerrainContact.surface.mu = 255.0f; + TerrainContact.surface.bounce = 0.0f; + TerrainContact.surface.soft_cfm = 0.0f; + TerrainContact.surface.soft_erp = 0.05f; WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM); - WaterContact.surface.mu = 0f; // No friction + WaterContact.surface.mu = 0.0f; // No friction WaterContact.surface.bounce = 0.0f; // No bounce WaterContact.surface.soft_cfm = 0.010f; WaterContact.surface.soft_erp = 0.010f; // Prim contact friction and bounce - // THis is the *non* moving version of friction and bounce + // THis is the moving version of friction and bounce // Use this when an avatar comes in contact with a prim - // and is moving - AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction; - AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce; + AvatarMovementprimContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + AvatarMovementprimContact.surface.mu = 255.0f; + AvatarMovementprimContact.surface.bounce = 0.01f; + AvatarMovementprimContact.surface.soft_cfm = 0.0f; // if this is 0.01 then prims become phantom to Avs! + AvatarMovementprimContact.surface.soft_erp = 0.001f; // Terrain contact friction bounce and various error correcting calculations // Use this when an avatar is in contact with the terrain and moving. +/* AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP; AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction; AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce; AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP; - +*/ + AvatarMovementTerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + AvatarMovementTerrainContact.surface.mu = 75f; + AvatarMovementTerrainContact.surface.bounce = 0.0f; + AvatarMovementTerrainContact.surface.soft_cfm = 0.0f; + AvatarMovementTerrainContact.surface.soft_erp = 0.05f; /* @@ -549,109 +561,110 @@ namespace OpenSim.Region.Physics.OdePlugin m_materialContacts = new d.Contact[7,2]; m_materialContacts[(int)Material.Stone, 0] = new d.Contact(); - m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Stone, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Stone, 0].surface.mu = 1.8f; + m_materialContacts[(int)Material.Stone, 0].surface.bounce = 0.0f; + m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Stone, 1] = new d.Contact(); - m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Stone, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Stone, 1].surface.mu = 1.8f; + m_materialContacts[(int)Material.Stone, 1].surface.bounce = 0.0f; + m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Metal, 0] = new d.Contact(); - m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Metal, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Metal, 0].surface.mu = 1.3f; + m_materialContacts[(int)Material.Metal, 0].surface.bounce = 0.2f; + m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Metal, 1] = new d.Contact(); - m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Glass, 0] = new d.Contact(); - m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f; - m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f; - m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Metal, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Metal, 1].surface.mu = 1.3f; + m_materialContacts[(int)Material.Metal, 1].surface.bounce = 0.2f; + m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.50f; /* - private float nmAvatarObjectContactFriction = 250f; + flags : d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce + private float nmAvatarObjectContactFriction = 250f; private float nmAvatarObjectContactBounce = 0.1f; private float mAvatarObjectContactFriction = 75f; private float mAvatarObjectContactBounce = 0.1f; */ + m_materialContacts[(int)Material.Glass, 0] = new d.Contact(); + m_materialContacts[(int)Material.Glass, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f; + m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.1f; + m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.50f; + m_materialContacts[(int)Material.Glass, 1] = new d.Contact(); - m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Glass, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f; - m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f; - m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.1f; + m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Wood, 0] = new d.Contact(); - m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Wood, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Wood, 0].surface.mu = 1.6f; + m_materialContacts[(int)Material.Wood, 0].surface.bounce = 0.1f; + m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Wood, 1] = new d.Contact(); - m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Wood, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Wood, 1].surface.mu = 1.6f; + m_materialContacts[(int)Material.Wood, 1].surface.bounce = 0.1f; + m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Flesh, 0] = new d.Contact(); - m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Flesh, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Flesh, 0].surface.mu = 2.0f; + m_materialContacts[(int)Material.Flesh, 0].surface.bounce = 0.0f; + m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Flesh, 1] = new d.Contact(); - m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Flesh, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Flesh, 1].surface.mu = 2.0f; + m_materialContacts[(int)Material.Flesh, 1].surface.bounce = 0.0f; + m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Plastic, 0] = new d.Contact(); - m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Plastic, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Plastic, 0].surface.mu = 1.5f; + m_materialContacts[(int)Material.Plastic, 0].surface.bounce = 0.2f; + m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Plastic, 1] = new d.Contact(); - m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Plastic, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Plastic, 1].surface.mu = 1.5f; + m_materialContacts[(int)Material.Plastic, 1].surface.bounce = 0.2f; + m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Rubber, 0] = new d.Contact(); - m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Rubber, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Rubber, 0].surface.mu = 2.0f; + m_materialContacts[(int)Material.Rubber, 0].surface.bounce = 0.7f; + m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Rubber, 1] = new d.Contact(); - m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f; + m_materialContacts[(int)Material.Rubber, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + m_materialContacts[(int)Material.Rubber, 1].surface.mu = 2.0f; + m_materialContacts[(int)Material.Rubber, 1].surface.bounce = 0.7f; + m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.50f; d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh); @@ -660,10 +673,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldSetGravity(world, gravityx, gravityy, gravityz); d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); + + d.WorldSetLinearDampingThreshold(world, 256f); d.WorldSetLinearDamping(world, 256f); - d.WorldSetAngularDamping(world, 256f); +// d.WorldSetLinearDampingThreshold(world, 0.01f); +// d.WorldSetLinearDamping(world, 0.1f); d.WorldSetAngularDampingThreshold(world, 256f); - d.WorldSetLinearDampingThreshold(world, 256f); + d.WorldSetAngularDamping(world, 256f); d.WorldSetMaxAngularSpeed(world, 256f); // Set how many steps we go without running collision testing @@ -1184,7 +1200,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) { - // Use the Movement prim contact + // Use the AV Movement / prim contact AvatarMovementprimContact.geom = curContact; _perloopContact.Add(curContact); if (m_global_contactcount < maxContactsbeforedeath) @@ -1195,7 +1211,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - // Use the non movement contact + // Use the Av non movement / prim contact contact.geom = curContact; _perloopContact.Add(curContact); -- cgit v1.1 From 64209c9be1cc8c88bdf8f5c9cb3c1ddebd8f23d2 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Wed, 29 Dec 2010 21:13:49 +0000 Subject: Fix av/prim eject problem. --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 105 ++++++++++++++---------- 1 file changed, 60 insertions(+), 45 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 756c005..e7455be 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -496,7 +496,7 @@ namespace OpenSim.Region.Physics.OdePlugin contact.surface.mu = 255.0f; contact.surface.bounce = 0.0f; contact.surface.soft_cfm = 0.0f; - contact.surface.soft_erp = 1.00f; // If this is too small static Av will fall through a sloping prim. + contact.surface.soft_erp = 0.30f; // If this is too small static Av will fall through a sloping prim. 1.0 prevents fall-thru // Terrain contact friction and Bounce // This is the *non* moving version. Use this when an avatar @@ -523,9 +523,9 @@ namespace OpenSim.Region.Physics.OdePlugin // Use this when an avatar comes in contact with a prim AvatarMovementprimContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; AvatarMovementprimContact.surface.mu = 255.0f; - AvatarMovementprimContact.surface.bounce = 0.01f; + AvatarMovementprimContact.surface.bounce = 0.0f; AvatarMovementprimContact.surface.soft_cfm = 0.0f; // if this is 0.01 then prims become phantom to Avs! - AvatarMovementprimContact.surface.soft_erp = 0.001f; + AvatarMovementprimContact.surface.soft_erp = 0.3f; // Terrain contact friction bounce and various error correcting calculations // Use this when an avatar is in contact with the terrain and moving. @@ -559,11 +559,11 @@ namespace OpenSim.Region.Physics.OdePlugin */ m_materialContacts = new d.Contact[7,2]; - + // V 1 = Sliding; 0 = static or fell onto m_materialContacts[(int)Material.Stone, 0] = new d.Contact(); m_materialContacts[(int)Material.Stone, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Stone, 0].surface.mu = 1.8f; - m_materialContacts[(int)Material.Stone, 0].surface.bounce = 0.0f; + m_materialContacts[(int)Material.Stone, 0].surface.mu = 1.8f; // friction, 1 = slippery, 255 = no slip + m_materialContacts[(int)Material.Stone, 0].surface.bounce = 0.0f; m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.50f; @@ -588,27 +588,19 @@ namespace OpenSim.Region.Physics.OdePlugin m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.50f; - /* - flags : d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce - private float nmAvatarObjectContactFriction = 250f; - private float nmAvatarObjectContactBounce = 0.1f; - - private float mAvatarObjectContactFriction = 75f; - private float mAvatarObjectContactBounce = 0.1f; - */ m_materialContacts[(int)Material.Glass, 0] = new d.Contact(); m_materialContacts[(int)Material.Glass, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f; - m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.1f; - m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.0f; + m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.0f; + m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.01f; m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Glass, 1] = new d.Contact(); m_materialContacts[(int)Material.Glass, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f; - m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.1f; + m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.0f; m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.0f; - m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.50f; + m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.30f; m_materialContacts[(int)Material.Wood, 0] = new d.Contact(); m_materialContacts[(int)Material.Wood, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; @@ -827,6 +819,11 @@ namespace OpenSim.Region.Physics.OdePlugin p2 = PANull; } +if((p1 is OdePrim ) && (p2 is OdePrim)){ + OdePrim t1 = (OdePrim)p1; + OdePrim t2 = (OdePrim)p2; + Console.WriteLine("Collision {0} {1}", t1.m_primName, t2.m_primName); +} ContactPoint maxDepthContact = new ContactPoint(); if (p1.CollisionScore + count >= float.MaxValue) p1.CollisionScore = 0; @@ -835,7 +832,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (p2.CollisionScore + count >= float.MaxValue) p2.CollisionScore = 0; p2.CollisionScore += count; - for (int i = 0; i < count; i++) { d.ContactGeom curContact = contacts[i]; @@ -863,18 +859,13 @@ namespace OpenSim.Region.Physics.OdePlugin //#@ if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)) //#@ p2.IsColliding = true; if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)){ //## -//Console.WriteLine("AvColl 1 {0} - {1} - {2} - {3}", //## -// curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); //## p2.IsColliding = true; //## }else{ -//Console.WriteLine("AvColl 2 {0} - {1} - {2} - {3}", //## -// curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); //## } //## } else { -//Console.WriteLine("AvColl 3 {0}", p2.PhysicsActorType); //## p2.IsColliding = true; } @@ -1075,6 +1066,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (!skipThisContact) { + // Add contact joints with materials params---------------------------------- + int material = (int) Material.Wood; + int movintYN = 0; // 1 = Sliding; 0 = static or fell onto + // If we're colliding against terrain if (name1 == "Terrain" || name2 == "Terrain") { @@ -1082,7 +1077,7 @@ namespace OpenSim.Region.Physics.OdePlugin if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) { - // Use the movement terrain contact + //$ Av walk/run on terrain (not falling) Use the Avatar movement terrain contact AvatarMovementTerrainContact.geom = curContact; _perloopContact.Add(curContact); if (m_global_contactcount < maxContactsbeforedeath) @@ -1095,7 +1090,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (p2.PhysicsActorType == (int)ActorTypes.Agent) { - // Use the non moving terrain contact + //$ Av standing on terrain, Use the non moving Avata terrain contact TerrainContact.geom = curContact; _perloopContact.Add(curContact); if (m_global_contactcount < maxContactsbeforedeath) @@ -1108,10 +1103,8 @@ namespace OpenSim.Region.Physics.OdePlugin { if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim) { - // prim prim contact + //& THIS NEVER HAPPENS prim prim contact //kf Huh? In terrain contact? // int pj294950 = 0; - int movintYN = 0; - int material = (int) Material.Wood; // prim terrain contact if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) { @@ -1120,7 +1113,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (p2 is OdePrim) material = ((OdePrim)p2).m_material; - //m_log.DebugFormat("Material: {0}", material); m_materialContacts[material, movintYN].geom = curContact; _perloopContact.Add(curContact); @@ -1135,16 +1127,12 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - - int movintYN = 0; - // prim terrain contact + //$ prim on terrain contact if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) { movintYN = 1; } - int material = (int)Material.Wood; - if (p2 is OdePrim) material = ((OdePrim)p2).m_material; //m_log.DebugFormat("Material: {0}", material); @@ -1167,6 +1155,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else if (name1 == "Water" || name2 == "Water") { + //$ This never happens! /* if ((p2.PhysicsActorType == (int) ActorTypes.Prim)) { @@ -1194,10 +1183,12 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - // we're colliding with prim or avatar + + // no terrain and no water, we're colliding with prim or avatar // check if we're moving if ((p2.PhysicsActorType == (int)ActorTypes.Agent)) { + //$ Avatar on Prim or other Avatar if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) { // Use the AV Movement / prim contact @@ -1217,28 +1208,52 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_global_contactcount < maxContactsbeforedeath) { + if (curContact.depth > 0.2) + { // embedded, eject slowly + contact.surface.soft_erp = 0.1f; + contact.surface.soft_cfm = 0.1f; + } + else + { // keep on the surface + contact.surface.soft_erp = 0.3f; + contact.surface.soft_cfm = 0.0f; + } joint = d.JointCreateContact(world, contactgroup, ref contact); m_global_contactcount++; +/* +Console.WriteLine("Prim | Av collision 2 mode={0} mu={1} bounce={2} bv={3} erp={4} cfm={5} mot={6} depth={7}", +contact.surface.mode, +contact.surface.mu, +contact.surface.bounce, +contact.surface.bounce_vel, +contact.surface.soft_erp, +contact.surface.soft_cfm, +contact.surface.motion1, +curContact.depth); +*/ } } } else if (p2.PhysicsActorType == (int)ActorTypes.Prim) { + //$ Prim on Prim //p1.PhysicsActorType - int material = (int)Material.Wood; - if (p2 is OdePrim) - material = ((OdePrim)p2).m_material; - + if (p2 is OdePrim) material = ((OdePrim)p2).m_material; //m_log.DebugFormat("Material: {0}", material); - m_materialContacts[material, 0].geom = curContact; + + if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) + { + movintYN = 1; + } + + m_materialContacts[material, movintYN].geom = curContact; _perloopContact.Add(curContact); if (m_global_contactcount < maxContactsbeforedeath) { - joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]); + joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); m_global_contactcount++; - } } } @@ -1261,8 +1276,8 @@ namespace OpenSim.Region.Physics.OdePlugin } //m_log.Debug(count.ToString()); //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); - } - } + } // end for i.. loop + } // end near private bool checkDupe(d.ContactGeom contactGeom, int atype) { -- cgit v1.1 From ba7a2277633804eb2d5e449efa0895e9f43a4ece Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Tue, 4 Jan 2011 21:36:09 +0000 Subject: Revise Materials properties; Fix Double-Click Autopilot; Allow non-script sit positions >= 0.1M; Add llLookAt(); Comment out spammy bad adjacent sim message. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 216 +++++++++++++---------- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 220 +++++++++--------------- 2 files changed, 209 insertions(+), 227 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 16e90ff..8402082 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -85,6 +85,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_APIDStrength = 0.5f; private float m_APIDDamping = 0.5f; private bool m_useAPID = false; + private float m_APIDdamper = 1.0f; // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), // do not confuse with VEHICLE HOVER @@ -735,12 +736,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override float Buoyancy { get { return m_buoyancy; } -// set { m_buoyancy = value; } - set { - m_buoyancy = value; - - Console.WriteLine("m_buoyancy={0}", m_buoyancy); - } + set { m_buoyancy = value; } } public override void link(PhysicsActor obj) @@ -1293,7 +1289,6 @@ namespace OpenSim.Region.Physics.OdePlugin disableBody(); } } - IntPtr vertices, indices; int vertexCount, indexCount; int vertexStride, triStride; @@ -1349,8 +1344,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (prim_geom != IntPtr.Zero) { if (!_position.ApproxEquals(m_taintposition, 0f)) + { changemove(timestep); - + } if (m_taintrot != _orientation) { if(childPrim && IsPhysical) // For physical child prim... @@ -1370,8 +1366,9 @@ namespace OpenSim.Region.Physics.OdePlugin // if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) + { changePhysicsStatus(timestep); - // + }// if (!_size.ApproxEquals(m_taintsize,0f)) changesize(timestep); @@ -1482,6 +1479,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body == IntPtr.Zero) { Body = d.BodyCreate(_parent_scene.world); + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode (Body, false); + setMass(); } if (Body != IntPtr.Zero) @@ -1522,7 +1522,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); continue; } -//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); @@ -1565,9 +1564,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionCategories |= CollisionCategories.Body; m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); -//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); -//Console.WriteLine(" Post GeomSetCategoryBits 2"); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); @@ -1721,14 +1718,15 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical) { disableBodySoft(); - - if (Body != IntPtr.Zero) - { - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0, 0, 0); - enableBodySoft(); - } } + if (Body != IntPtr.Zero) + { + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); + } + } else { @@ -1749,17 +1747,21 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } -/* Uhhh - stop the motion if the object is _selected_!! + if (Body != IntPtr.Zero) + { + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); + } + if (m_isphysical) { if (Body != IntPtr.Zero) { - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0, 0, 0); enableBodySoft(); } } -*/ } resetCollisionAccounting(); @@ -1781,7 +1783,6 @@ namespace OpenSim.Region.Physics.OdePlugin public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) { -//Console.WriteLine("CreateGeom:"); if (_mesh != null) // Special - make mesh { setMesh(_parent_scene, _mesh); @@ -1797,7 +1798,6 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 1"); SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); } catch (AccessViolationException) @@ -1812,7 +1812,6 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 2"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1828,7 +1827,6 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 3"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1845,7 +1843,6 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 4"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1890,7 +1887,6 @@ namespace OpenSim.Region.Physics.OdePlugin lock (_parent_scene.OdeLock) { -//Console.WriteLine("changeadd 1"); CreateGeom(m_targetSpace, _mesh); if (prim_geom != IntPtr.Zero) @@ -1917,10 +1913,8 @@ namespace OpenSim.Region.Physics.OdePlugin public void changemove(float timestep) { -//Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position ); if (m_isphysical) { -//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim); // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! if (!m_taintremove && !childPrim) { @@ -1946,7 +1940,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) { // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? -Console.WriteLine(" JointCreateFixed"); +Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); d.JointAttach(m_linkJoint, Body, odParent.Body); d.JointSetFixed(m_linkJoint); @@ -2059,7 +2053,6 @@ Console.WriteLine(" JointCreateFixed"); if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) { _mesh = null; -//Console.WriteLine("changePhysicsStatus for " + m_primName ); changeadd(2f); } if (childPrim) @@ -2142,7 +2135,6 @@ Console.WriteLine(" JointCreateFixed"); } //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); -//Console.WriteLine("changesize 1"); CreateGeom(m_targetSpace, mesh); @@ -2150,7 +2142,6 @@ Console.WriteLine(" JointCreateFixed"); else { _mesh = null; -//Console.WriteLine("changesize 2"); CreateGeom(m_targetSpace, _mesh); } @@ -2255,7 +2246,6 @@ Console.WriteLine(" JointCreateFixed"); else { _mesh = null; -//Console.WriteLine("changeshape"); CreateGeom(m_targetSpace, null); } @@ -2330,6 +2320,7 @@ Console.WriteLine(" JointCreateFixed"); return; } d.BodyEnable(Body); + d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); } m_forcelist.Clear(); @@ -3107,8 +3098,6 @@ Console.WriteLine(" JointCreateFixed"); else { // Too many tries if (_parent == null) base.RaiseOutOfBounds(l_position); -//Console.WriteLine("ROOB 2"); - return; // Dont process any other motion? } // end various methods } // end outside region horizontally @@ -3124,7 +3113,6 @@ Console.WriteLine(" JointCreateFixed"); //IsPhysical = false; if (_parent == null) base.RaiseOutOfBounds(_position); -//Console.WriteLine("ROOB 3"); _acceleration.X = 0; // This stuff may stop client display but it has no @@ -3147,10 +3135,12 @@ Console.WriteLine(" JointCreateFixed"); } // end neg Z check // Is it moving? - if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) - && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) - && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // KF 0.01 is far to large + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ + if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly + (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating + (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly { _zeroFlag = true; m_throttleUpdates = false; @@ -3167,15 +3157,19 @@ Console.WriteLine(" JointCreateFixed"); { // Its stopped _velocity.X = 0.0f; _velocity.Y = 0.0f; - _velocity.Z = 0.0f; + // _velocity.Z = 0.0f; _acceleration.X = 0; _acceleration.Y = 0; - _acceleration.Z = 0; + // _acceleration.Z = 0; m_rotationalVelocity.X = 0; m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; + // Stop it in the phys engine + d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); + d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + if (!m_lastUpdateSent) { m_throttleUpdates = false; @@ -3221,13 +3215,13 @@ Console.WriteLine(" JointCreateFixed"); // Snapshot current angles, set up Amotor(s) m_rotateEnableUpdate = false; m_rotateEnable = m_rotateEnableRequest; -Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); +//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); if (Amotor != IntPtr.Zero) { d.JointDestroy(Amotor); Amotor = IntPtr.Zero; -Console.WriteLine("Old Amotor Destroyed"); +//Console.WriteLine("Old Amotor Destroyed"); } if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) @@ -3249,33 +3243,33 @@ Console.WriteLine("Old Amotor Destroyed"); Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); d.JointAttach(Amotor, Body, IntPtr.Zero); d.JointSetAMotorMode(Amotor, 0); // User mode?? -Console.WriteLine("New Amotor Created for {0}", m_primName); +//Console.WriteLine("New Amotor Created for {0}", m_primName); float axisnum = 3; // how many to lock axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); d.JointSetAMotorNumAxes(Amotor,(int)axisnum); -Console.WriteLine("AxisNum={0}",(int)axisnum); +//Console.WriteLine("AxisNum={0}",(int)axisnum); int i = 0; if (m_rotateEnable.X == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); -Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); +//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); i++; } if (m_rotateEnable.Y == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); -Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); +//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); i++; } if (m_rotateEnable.Z == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); -Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); +//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); i++; } @@ -3308,7 +3302,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); m_lLinObjectVel = vel_now * irotq; - if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate { if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) @@ -3434,7 +3427,7 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); // apply gravity force d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); -//if(frcount == 0) Console.WriteLine("Grav {0}", grav); +//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); // end MoveLinear() @@ -3608,7 +3601,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); float m_mass = CalculateMass(); // calculate z-force due togravity on object. fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass - if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. { fz = 0; // llMoveToTarget ignores gravity. @@ -3624,6 +3616,7 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); (m_PIDTarget.Z - pos.Z)); if (error.ApproxEquals(Vector3.Zero,0.01f)) { // Very close, Jump there and quit move + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); _target_velocity = Vector3.Zero; d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); @@ -3714,11 +3707,10 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); // Avatar to Avatar collisions // Prim to avatar collisions d.Vector3 dlinvel = vel; - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); d.BodyAddForce(Body, 0, 0, fz); - //KF this prevents furthur motions return; + //KF this prevents furthur motions return; } else { @@ -3734,7 +3726,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); fx *= m_mass; fy *= m_mass; //fz *= m_mass; - fx += m_force.X; fy += m_force.Y; fz += m_force.Z; @@ -3751,7 +3742,7 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); // this appears to re-enable it incase the surface it is upon vanishes, // and the body should fall again. d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0, 0, 0); + d.BodySetForce(Body, 0f, 0f, 0f); enableBodySoft(); } @@ -3769,11 +3760,10 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); if (fy < nmin) fy = nmin; d.BodyAddForce(Body, fx, fy, fz); -//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); } // end apply forces } // end Vehicle/Dynamics - /// RotLookAt ================================================================================= + /// RotLookAt / LookAt ================================================================================= if (m_useAPID) { // RotLookAt, apparently overrides all other rotation sources. Inputs: @@ -3784,41 +3774,87 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); // Factors: // get present body rotation float limit = 1.0f; - float scaler = 50f; // adjusts damping time + float rscaler = 50f; // adjusts rotation damping time + float lscaler = 10f; // adjusts linear damping time in llLookAt float RLAservo = 0f; - - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; - float diff_angle; Vector3 diff_axis; - rot_diff.GetAxisAngle(out diff_axis, out diff_angle); - diff_axis.Normalize(); - if(diff_angle > 0.01f) // diff_angle is always +ve + float diff_angle; + d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rtarget = new Quaternion(); + + if(m_APIDTarget.W == -99.9f) { -// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); - Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); - rotforce = rotforce * rotq; - if(diff_angle > limit) diff_angle = limit; // cap the rotate rate -// RLAservo = timestep / m_APIDStrength * m_mass * scaler; - // rotforce = rotforce * RLAservo * diff_angle ; - // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); - RLAservo = timestep / m_APIDStrength * scaler; - rotforce = rotforce * RLAservo * diff_angle ; - /* - if (m_angularEnable.X == 0) - rotforce.X = 0; - if (m_angularEnable.Y == 0) - rotforce.Y = 0; - if (m_angularEnable.Z == 0) - rotforce.Z = 0; - */ - d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); -//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + // this is really a llLookAt(), x,y,z is the target vector + Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); + Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; + Vector3 error = new Vector3(0.0f, 0.0f, 0.0f); + float twopi = 2.0f * (float)Math.PI; + Vector3 dir = target - _position; + dir.Normalize(); + float tzrot = (float)Math.Atan2(dir.Y, dir.X); + float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y)); + float terot = (float)Math.Atan2(dir.Z, txy); + float ozrot = (float)Math.Atan2(ospin.Y, ospin.X); + float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y)); + float oerot = (float)Math.Atan2(ospin.Z, oxy); + float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z)); + float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X)); + float roll = (float)Math.Atan2(ra, rb); + float errorz = tzrot - ozrot; + if(errorz > (float)Math.PI) errorz -= twopi; + else if(errorz < -(float)Math.PI) errorz += twopi; + float errory = oerot - terot; + if(errory > (float)Math.PI) errory -= twopi; + else if(errory < -(float)Math.PI) errory += twopi; + diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll); + if(diff_angle > 0.01f * m_APIDdamper) + { + m_APIDdamper = 1.0f; + RLAservo = timestep / m_APIDStrength * rscaler; + errorz *= RLAservo; + errory *= RLAservo; + error.X = -roll * 8.0f; + error.Y = errory; + error.Z = errorz; + error *= rotq; + d.BodySetAngularVel (Body, error.X, error.Y, error.Z); + } + else + { + d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + m_APIDdamper = 2.0f; + } } -//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); + else + { + // this is a llRotLookAt() + rtarget = m_APIDTarget; + + Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle +//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); + + // diff_axis.Normalize(); it already is! + if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error + { + m_APIDdamper = 1.0f; + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if(diff_angle > limit) diff_angle = limit; // cap the rotate rate + RLAservo = timestep / m_APIDStrength * lscaler; + rotforce = rotforce * RLAservo * diff_angle ; + d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); +//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } + else + { // close enough + d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + m_APIDdamper = 2.0f; + } + } // end llLookAt/llRotLookAt +//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); } // end m_useAPID - } // end root prims } // end Move() } // end class diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index e7455be..88f9658 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -264,8 +264,9 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly DoubleDictionary RegionTerrain = new DoubleDictionary(); private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); - private d.Contact contact; + private d.Contact ContactCopy; // local copy that can be modified private d.Contact TerrainContact; + private d.Contact AvatarStaticprimContact; // was 'contact' private d.Contact AvatarMovementprimContact; private d.Contact AvatarMovementTerrainContact; private d.Contact WaterContact; @@ -491,169 +492,140 @@ namespace OpenSim.Region.Physics.OdePlugin staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)]; - // Centeral contact friction and bounce // KF: This appears to be only for static AV on non-phys prim. - contact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - contact.surface.mu = 255.0f; - contact.surface.bounce = 0.0f; - contact.surface.soft_cfm = 0.0f; - contact.surface.soft_erp = 0.30f; // If this is too small static Av will fall through a sloping prim. 1.0 prevents fall-thru - - // Terrain contact friction and Bounce - // This is the *non* moving version. Use this when an avatar - // isn't moving to keep it in place better -/* TerrainContact.surface.mode |= d.ContactFlags.SoftERP; - TerrainContact.surface.mu = nmTerrainContactFriction; - TerrainContact.surface.bounce = nmTerrainContactBounce; - TerrainContact.surface.soft_erp = nmTerrainContactERP; */ + // Avatar static on a Prim parameters + AvatarStaticprimContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + AvatarStaticprimContact.surface.mu = 255.0f; + AvatarStaticprimContact.surface.bounce = 0.0f; + AvatarStaticprimContact.surface.soft_cfm = 0.0f; + AvatarStaticprimContact.surface.soft_erp = 0.30f; // If this is too small static Av will fall through a sloping prim. 1.0 prevents fall-thru - TerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - TerrainContact.surface.mu = 255.0f; - TerrainContact.surface.bounce = 0.0f; - TerrainContact.surface.soft_cfm = 0.0f; - TerrainContact.surface.soft_erp = 0.05f; - - WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM); - WaterContact.surface.mu = 0.0f; // No friction - WaterContact.surface.bounce = 0.0f; // No bounce - WaterContact.surface.soft_cfm = 0.010f; - WaterContact.surface.soft_erp = 0.010f; - - // Prim contact friction and bounce - // THis is the moving version of friction and bounce - // Use this when an avatar comes in contact with a prim + // Avatar moving on a Prim parameters AvatarMovementprimContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; AvatarMovementprimContact.surface.mu = 255.0f; AvatarMovementprimContact.surface.bounce = 0.0f; AvatarMovementprimContact.surface.soft_cfm = 0.0f; // if this is 0.01 then prims become phantom to Avs! AvatarMovementprimContact.surface.soft_erp = 0.3f; - // Terrain contact friction bounce and various error correcting calculations - // Use this when an avatar is in contact with the terrain and moving. -/* - AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP; - AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction; - AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce; - AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP; -*/ + // Static Avatar on Terrain parameters + // Keeps Avatar in place better + TerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; + TerrainContact.surface.mu = 255.0f; + TerrainContact.surface.bounce = 0.0f; + TerrainContact.surface.soft_cfm = 0.0f; + TerrainContact.surface.soft_erp = 0.05f; + + // Moving Avatar on Terrain parameters AvatarMovementTerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; AvatarMovementTerrainContact.surface.mu = 75f; AvatarMovementTerrainContact.surface.bounce = 0.0f; AvatarMovementTerrainContact.surface.soft_cfm = 0.0f; AvatarMovementTerrainContact.surface.soft_erp = 0.05f; - /* - - Stone = 0, - /// - Metal = 1, - /// - Glass = 2, - /// - Wood = 3, - /// - Flesh = 4, - /// - Plastic = 5, - /// - Rubber = 6 - */ + // Avatar or prim the the water, this may not be used, possibly water is same as air? + WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM); + WaterContact.surface.mu = 0.0f; // No friction + WaterContact.surface.bounce = 0.0f; // No bounce + WaterContact.surface.soft_cfm = 0.010f; + WaterContact.surface.soft_erp = 0.010f; + + // Prim static or moving on a prim, depends on material type m_materialContacts = new d.Contact[7,2]; // V 1 = Sliding; 0 = static or fell onto m_materialContacts[(int)Material.Stone, 0] = new d.Contact(); m_materialContacts[(int)Material.Stone, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Stone, 0].surface.mu = 1.8f; // friction, 1 = slippery, 255 = no slip + m_materialContacts[(int)Material.Stone, 0].surface.mu = 60f; // friction, 1 = slippery, 255 = no slip m_materialContacts[(int)Material.Stone, 0].surface.bounce = 0.0f; m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.0f; - m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.50f; + m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.50f; // erp also changes friction, more erp=less friction m_materialContacts[(int)Material.Stone, 1] = new d.Contact(); m_materialContacts[(int)Material.Stone, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Stone, 1].surface.mu = 1.8f; + m_materialContacts[(int)Material.Stone, 1].surface.mu = 40f; m_materialContacts[(int)Material.Stone, 1].surface.bounce = 0.0f; m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Metal, 0] = new d.Contact(); m_materialContacts[(int)Material.Metal, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Metal, 0].surface.mu = 1.3f; + m_materialContacts[(int)Material.Metal, 0].surface.mu = 15f; m_materialContacts[(int)Material.Metal, 0].surface.bounce = 0.2f; m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Metal, 1] = new d.Contact(); m_materialContacts[(int)Material.Metal, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Metal, 1].surface.mu = 1.3f; + m_materialContacts[(int)Material.Metal, 1].surface.mu = 10f; m_materialContacts[(int)Material.Metal, 1].surface.bounce = 0.2f; m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Glass, 0] = new d.Contact(); m_materialContacts[(int)Material.Glass, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f; + m_materialContacts[(int)Material.Glass, 0].surface.mu = 7.5f; m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.0f; - m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.01f; + m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Glass, 1] = new d.Contact(); m_materialContacts[(int)Material.Glass, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f; + m_materialContacts[(int)Material.Glass, 1].surface.mu = 5f; m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.0f; m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.0f; - m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.30f; + m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Wood, 0] = new d.Contact(); m_materialContacts[(int)Material.Wood, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Wood, 0].surface.mu = 1.6f; + m_materialContacts[(int)Material.Wood, 0].surface.mu = 45f; m_materialContacts[(int)Material.Wood, 0].surface.bounce = 0.1f; m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Wood, 1] = new d.Contact(); m_materialContacts[(int)Material.Wood, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Wood, 1].surface.mu = 1.6f; + m_materialContacts[(int)Material.Wood, 1].surface.mu = 30f; m_materialContacts[(int)Material.Wood, 1].surface.bounce = 0.1f; m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Flesh, 0] = new d.Contact(); m_materialContacts[(int)Material.Flesh, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Flesh, 0].surface.mu = 2.0f; + m_materialContacts[(int)Material.Flesh, 0].surface.mu = 150f; m_materialContacts[(int)Material.Flesh, 0].surface.bounce = 0.0f; m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Flesh, 1] = new d.Contact(); m_materialContacts[(int)Material.Flesh, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Flesh, 1].surface.mu = 2.0f; + m_materialContacts[(int)Material.Flesh, 1].surface.mu = 100f; m_materialContacts[(int)Material.Flesh, 1].surface.bounce = 0.0f; m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Plastic, 0] = new d.Contact(); m_materialContacts[(int)Material.Plastic, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Plastic, 0].surface.mu = 1.5f; + m_materialContacts[(int)Material.Plastic, 0].surface.mu = 30f; m_materialContacts[(int)Material.Plastic, 0].surface.bounce = 0.2f; m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Plastic, 1] = new d.Contact(); m_materialContacts[(int)Material.Plastic, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Plastic, 1].surface.mu = 1.5f; + m_materialContacts[(int)Material.Plastic, 1].surface.mu = 20f; m_materialContacts[(int)Material.Plastic, 1].surface.bounce = 0.2f; m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Rubber, 0] = new d.Contact(); m_materialContacts[(int)Material.Rubber, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Rubber, 0].surface.mu = 2.0f; + m_materialContacts[(int)Material.Rubber, 0].surface.mu = 150f; m_materialContacts[(int)Material.Rubber, 0].surface.bounce = 0.7f; m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.50f; m_materialContacts[(int)Material.Rubber, 1] = new d.Contact(); m_materialContacts[(int)Material.Rubber, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce; - m_materialContacts[(int)Material.Rubber, 1].surface.mu = 2.0f; + m_materialContacts[(int)Material.Rubber, 1].surface.mu = 100f; m_materialContacts[(int)Material.Rubber, 1].surface.bounce = 0.7f; m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.0f; m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.50f; @@ -819,11 +791,6 @@ namespace OpenSim.Region.Physics.OdePlugin p2 = PANull; } -if((p1 is OdePrim ) && (p2 is OdePrim)){ - OdePrim t1 = (OdePrim)p1; - OdePrim t2 = (OdePrim)p2; - Console.WriteLine("Collision {0} {1}", t1.m_primName, t2.m_primName); -} ContactPoint maxDepthContact = new ContactPoint(); if (p1.CollisionScore + count >= float.MaxValue) p1.CollisionScore = 0; @@ -847,7 +814,7 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ //m_log.Warn("[CCOUNT]: " + count); IntPtr joint; - // If we're colliding with terrain, use 'TerrainContact' instead of contact. + // If we're colliding with terrain, use 'TerrainContact' instead of AvatarStaticprimContact. // allows us to have different settings // We only need to test p2 for 'jump crouch purposes' @@ -898,9 +865,9 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ //This is disabled at the moment only because it needs more tweaking //It will eventually be uncommented /* - if (contact.depth >= 1.00f) + if (AvatarStaticprimContact.depth >= 1.00f) { - //m_log.Debug("[PHYSICS]: " + contact.depth.ToString()); + //m_log.Debug("[PHYSICS]: " + AvatarStaticprimContact.depth.ToString()); } //If you interpenetrate a prim with an agent @@ -910,37 +877,37 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ p2.PhysicsActorType == (int) ActorTypes.Prim)) { - //contact.depth = contact.depth * 4.15f; + //AvatarStaticprimContact.depth = AvatarStaticprimContact.depth * 4.15f; /* if (p2.PhysicsActorType == (int) ActorTypes.Agent) { p2.CollidingObj = true; - contact.depth = 0.003f; + AvatarStaticprimContact.depth = 0.003f; p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f); OdeCharacter character = (OdeCharacter) p2; character.SetPidStatus(true); - contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2)); + AvatarStaticprimContact.pos = new d.Vector3(AvatarStaticprimContact.pos.X + (p1.Size.X / 2), AvatarStaticprimContact.pos.Y + (p1.Size.Y / 2), AvatarStaticprimContact.pos.Z + (p1.Size.Z / 2)); } else { - //contact.depth = 0.0000000f; + //AvatarStaticprimContact.depth = 0.0000000f; } if (p1.PhysicsActorType == (int) ActorTypes.Agent) { p1.CollidingObj = true; - contact.depth = 0.003f; + AvatarStaticprimContact.depth = 0.003f; p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f); - contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2)); + AvatarStaticprimContact.pos = new d.Vector3(AvatarStaticprimContact.pos.X + (p2.Size.X / 2), AvatarStaticprimContact.pos.Y + (p2.Size.Y / 2), AvatarStaticprimContact.pos.Z + (p2.Size.Z / 2)); OdeCharacter character = (OdeCharacter)p1; character.SetPidStatus(true); } else { - //contact.depth = 0.0000000f; + //AvatarStaticprimContact.depth = 0.0000000f; } @@ -965,7 +932,7 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ //AddPhysicsActorTaint(p2); //} - //if (contact.depth >= 0.25f) + //if (AvatarStaticprimContact.depth >= 0.25f) //{ // Don't collide, one or both prim will expld. @@ -983,13 +950,13 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ //AddPhysicsActorTaint(p2); //} - //contact.depth = contact.depth / 8f; - //contact.normal = new d.Vector3(0, 0, 1); + //AvatarStaticprimContact.depth = AvatarStaticprimContact.depth / 8f; + //AvatarStaticprimContact.normal = new d.Vector3(0, 0, 1); //} //if (op1.m_disabled || op2.m_disabled) //{ //Manually disabled objects stay disabled - //contact.depth = 0f; + //AvatarStaticprimContact.depth = 0f; //} #endregion } @@ -997,7 +964,7 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ #endregion if (curContact.depth >= 1.00f) { - //m_log.Info("[P]: " + contact.depth.ToString()); + //m_log.Info("[P]: " + AvatarStaticprimContact.depth.ToString()); if ((p2.PhysicsActorType == (int) ActorTypes.Agent && p1.PhysicsActorType == (int) ActorTypes.Unknown) || (p1.PhysicsActorType == (int) ActorTypes.Agent && @@ -1067,15 +1034,16 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ if (!skipThisContact) { // Add contact joints with materials params---------------------------------- + // p1 is what is being hit, p2 is the physical object doing the hitting int material = (int) Material.Wood; int movintYN = 0; // 1 = Sliding; 0 = static or fell onto + if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) movintYN = 1; // If we're colliding against terrain if (name1 == "Terrain" || name2 == "Terrain") { // If we're moving - if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && - (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) + if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && (movintYN == 1)) { //$ Av walk/run on terrain (not falling) Use the Avatar movement terrain contact AvatarMovementTerrainContact.geom = curContact; @@ -1103,13 +1071,9 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ { if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim) { - //& THIS NEVER HAPPENS prim prim contact //kf Huh? In terrain contact? + //& THIS NEVER HAPPENS? prim prim contact In terrain contact? // int pj294950 = 0; // prim terrain contact - if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) - { - movintYN = 1; - } if (p2 is OdePrim) material = ((OdePrim)p2).m_material; @@ -1128,34 +1092,31 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ else { //$ prim on terrain contact - if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) - { - movintYN = 1; - } - if (p2 is OdePrim) material = ((OdePrim)p2).m_material; //m_log.DebugFormat("Material: {0}", material); m_materialContacts[material, movintYN].geom = curContact; _perloopContact.Add(curContact); + ContactCopy = m_materialContacts[material, movintYN]; + if(movintYN == 1) + { + // prevent excessive slide on terrain + ContactCopy.surface.mu = m_materialContacts[material, movintYN].surface.mu * 30.0f; + } + if (m_global_contactcount < maxContactsbeforedeath) { - joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); + joint = d.JointCreateContact(world, contactgroup, ref ContactCopy); m_global_contactcount++; - } } } } - //if (p2.PhysicsActorType == (int)ActorTypes.Prim) - //{ - //m_log.Debug("[PHYSICS]: prim contacting with ground"); - //} } else if (name1 == "Water" || name2 == "Water") { - //$ This never happens! + //$ This never happens! Perhaps water is treated like air? /* if ((p2.PhysicsActorType == (int) ActorTypes.Prim)) { @@ -1169,8 +1130,8 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ if (curContact.depth > 0.1f) { curContact.depth *= 52; - //contact.normal = new d.Vector3(0, 0, 1); - //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f); + //AvatarStaticprimContact.normal = new d.Vector3(0, 0, 1); + //AvatarStaticprimContact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f); } WaterContact.geom = curContact; _perloopContact.Add(curContact); @@ -1179,7 +1140,7 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ joint = d.JointCreateContact(world, contactgroup, ref WaterContact); m_global_contactcount++; } - //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth); + //m_log.Info("[PHYSICS]: Prim Water Contact" + AvatarStaticprimContact.depth); } else { @@ -1189,7 +1150,7 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ if ((p2.PhysicsActorType == (int)ActorTypes.Agent)) { //$ Avatar on Prim or other Avatar - if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) + if (movintYN == 1) { // Use the AV Movement / prim contact AvatarMovementprimContact.geom = curContact; @@ -1203,34 +1164,24 @@ if((p1 is OdePrim ) && (p2 is OdePrim)){ else { // Use the Av non movement / prim contact - contact.geom = curContact; + AvatarStaticprimContact.geom = curContact; _perloopContact.Add(curContact); + ContactCopy = AvatarStaticprimContact; // local copy so we can change locally if (m_global_contactcount < maxContactsbeforedeath) { if (curContact.depth > 0.2) { // embedded, eject slowly - contact.surface.soft_erp = 0.1f; - contact.surface.soft_cfm = 0.1f; + ContactCopy.surface.soft_erp = 0.1f; + ContactCopy.surface.soft_cfm = 0.1f; } else { // keep on the surface - contact.surface.soft_erp = 0.3f; - contact.surface.soft_cfm = 0.0f; + ContactCopy.surface.soft_erp = 0.3f; + ContactCopy.surface.soft_cfm = 0.0f; } - joint = d.JointCreateContact(world, contactgroup, ref contact); + joint = d.JointCreateContact(world, contactgroup, ref ContactCopy); m_global_contactcount++; -/* -Console.WriteLine("Prim | Av collision 2 mode={0} mu={1} bounce={2} bv={3} erp={4} cfm={5} mot={6} depth={7}", -contact.surface.mode, -contact.surface.mu, -contact.surface.bounce, -contact.surface.bounce_vel, -contact.surface.soft_erp, -contact.surface.soft_cfm, -contact.surface.motion1, -curContact.depth); -*/ } } } @@ -1241,16 +1192,11 @@ curContact.depth); if (p2 is OdePrim) material = ((OdePrim)p2).m_material; //m_log.DebugFormat("Material: {0}", material); - - if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) - { - movintYN = 1; - } m_materialContacts[material, movintYN].geom = curContact; _perloopContact.Add(curContact); - if (m_global_contactcount < maxContactsbeforedeath) + if (m_global_contactcount < maxContactsbeforedeath) { joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); m_global_contactcount++; -- cgit v1.1 From bfa5b7850cde25f679ea09f491cf357d6b2fee8f Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 20 Jan 2011 06:51:50 +0000 Subject: Fix drift of static prim. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 8402082..7cd2dd1 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -545,6 +545,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { _velocity = value; + if (_velocity.ApproxEquals(Vector3.Zero,0.001f)) + _acceleration = Vector3.Zero; m_taintVelocity = value; _parent_scene.AddPhysicsActorTaint(this); @@ -662,7 +664,14 @@ namespace OpenSim.Region.Physics.OdePlugin public override Vector3 Acceleration // client updates read data via here { - get { return _acceleration; } + get + { + if (_zeroFlag) + { + return Vector3.Zero; + } + return _acceleration; + } } @@ -3122,6 +3131,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); _velocity.X = 0; _velocity.Y = 0; _velocity.Z = 0; + m_lastVelocity = Vector3.Zero; m_rotationalVelocity.X = 0; m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; @@ -3169,6 +3179,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // Stop it in the phys engine d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + d.BodySetForce(Body, 0f, 0f, 0f); if (!m_lastUpdateSent) { @@ -3620,6 +3631,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); _target_velocity = Vector3.Zero; d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + d.BodySetForce(Body, 0f, 0f, 0f); } else { -- cgit v1.1 From 566eff17de31b4ffc1aef85c6047ce9e4e8bf5cd Mon Sep 17 00:00:00 2001 From: Melanie Date: Fri, 28 Jan 2011 01:46:30 +0100 Subject: Comment a very spammy debug message that was being output directly to console. My last fox that makes bumping into sim borders/bans work makes this spam endlessly. --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 93f9964..1bce760 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -594,7 +594,7 @@ namespace OpenSim.Region.Physics.OdePlugin Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); _parent_scene.geom_name_map[Shell] = m_name; _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; -Console.WriteLine("**** Create {2} Dicts: actor={0} name={1} height={3} rad={4}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, m_name, CAPSULE_LENGTH, CAPSULE_RADIUS); +//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1} height={3} rad={4}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, m_name, CAPSULE_LENGTH, CAPSULE_RADIUS); d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); d.GeomSetCollideBits(Shell, (int)m_collisionFlags); -- cgit v1.1 From 9c1f7995820d9d480450774575ac469ff5113b0d Mon Sep 17 00:00:00 2001 From: Melanie Date: Sun, 22 May 2011 21:01:31 +0200 Subject: Implement llGodLikeRezObject and llGetUsedMemory --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 88f9658..a622745 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1553,7 +1553,8 @@ namespace OpenSim.Region.Physics.OdePlugin removeprims = new List(); } removeprims.Add(chr); - m_log.Debug("[PHYSICS]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!"); + /// Commented this because it triggers on every bullet + //m_log.Debug("[PHYSICS]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!"); } } } -- cgit v1.1 From cc69d12d542cc7cea66e7b045b97c27c43d205bd Mon Sep 17 00:00:00 2001 From: Tom Date: Fri, 15 Jul 2011 11:12:10 -0700 Subject: Fix a raycast issue --- .../Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs | 13 +++++++++++-- .../Region/Physics/OdePlugin/ODERayCastRequestManager.cs | 11 +++++++++-- 2 files changed, 20 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs index 7314107..712029e 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs @@ -109,8 +109,17 @@ namespace OpenSim.Region.Physics.OdePlugin ODERayCastRequest[] reqs = m_PendingRequests.ToArray(); for (int i = 0; i < reqs.Length; i++) { - if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast - RayCast(reqs[i]); // if there isn't anyone to send results + try + { + if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast + RayCast(reqs[i]); // if there isn't anyone to send results + } + catch + { + //Fail silently + //This can genuinely happen because raycast requests are queued, and the actor may have + //been removed from the scene since it was queued + } } /* foreach (ODERayCastRequest req in m_PendingRequests) diff --git a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs index ba77dae..15ccddc 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs @@ -109,8 +109,15 @@ namespace OpenSim.Region.Physics.OdePlugin ODERayCastRequest[] reqs = m_PendingRequests.ToArray(); for (int i = 0; i < reqs.Length; i++) { - if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast - RayCast(reqs[i]); // if there isn't anyone to send results + try + { + if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast + RayCast(reqs[i]); // if there isn't anyone to send results + } + catch + { + //Fail silently + } } /* foreach (ODERayCastRequest req in m_PendingRequests) -- cgit v1.1 From bb402d0d95c934fbcd8b1c03e228ec1d0a14f14d Mon Sep 17 00:00:00 2001 From: Tom Date: Fri, 15 Jul 2011 12:08:40 -0700 Subject: Add localid support to ch0de properly --- .../Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs | 8 +------- .../Physics/BulletDotNETPlugin/BulletDotNETScene.cs | 7 +------ OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs | 8 +------- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 3 ++- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 14 ++++---------- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 19 +++++++------------ OpenSim/Region/Physics/OdePlugin/OdeScene.cs | 8 +------- .../Region/Physics/OdePlugin/Tests/ODETestClass.cs | 2 +- OpenSim/Region/Physics/POSPlugin/POSScene.cs | 8 +------- OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs | 8 +------- 10 files changed, 20 insertions(+), 65 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs index b6e1cb4..6c9d9ab 100644 --- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs +++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs @@ -84,13 +84,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { return null; } diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs index 6df213d..0d1bd82 100644 --- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs @@ -213,12 +213,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin return newPrim; } - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation) - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical) + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { PhysicsActor result; IMesh mesh = null; diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs index f4245b6..8e9edac 100644 --- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs +++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs @@ -626,13 +626,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position, - OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation) - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position, - OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, bool isPhysical) + OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, bool isPhysical, uint localid) { PhysicsActor result; diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 7cd2dd1..82f1b94 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -256,8 +256,9 @@ namespace OpenSim.Region.Physics.OdePlugin public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) + Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) { + m_localID = localid; ode = dode; if (!pos.IsFinite()) { diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index a622745..353db44 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1717,7 +1717,7 @@ namespace OpenSim.Region.Physics.OdePlugin } private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - IMesh mesh, PrimitiveBaseShape pbs, bool isphysical) + IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, uint localid) { Vector3 pos = position; @@ -1727,7 +1727,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode); + newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode, localid); lock (_prims) _prims.Add(newPrim); @@ -1749,13 +1749,7 @@ namespace OpenSim.Region.Physics.OdePlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) //To be removed - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { PhysicsActor result; IMesh mesh = null; @@ -1763,7 +1757,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (needsMeshing(pbs)) mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical); - result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical); + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, localid); return result; } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 13ea084..de22fae 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -77,15 +77,16 @@ namespace OpenSim.Region.Physics.Manager public abstract void RemovePrim(PhysicsActor prim); + //public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + // Vector3 size, Quaternion rotation); //To be removed - Actually removed! + public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation); //To be removed - public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical); + Vector3 size, Quaternion rotation, bool isPhysical, uint localid); public virtual PhysicsActor AddPrimShape(uint localID, string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { - PhysicsActor ret = AddPrimShape(primName, pbs, position, size, rotation, isPhysical); + PhysicsActor ret = AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid); if (ret != null) ret.LocalID = localID; @@ -262,13 +263,7 @@ namespace OpenSim.Region.Physics.Manager */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) //To be removed - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { m_log.InfoFormat("[PHYSICS]: NullPhysicsScene : AddPrim({0},{1})", position, size); return PhysicsActor.Null; diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs index f5172aa..88902b0 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs @@ -1708,13 +1708,7 @@ namespace OpenSim.Region.Physics.OdePlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) //To be removed - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { #if SPAM m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); diff --git a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs index a7f8baa..5dcd6f5 100644 --- a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs +++ b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs @@ -83,7 +83,7 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f); Vector3 size = new Vector3(0.5f, 0.5f, 0.5f); Quaternion rot = Quaternion.Identity; - PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true); + PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true, 0); OdePrim oprim = (OdePrim)prim; OdeScene pscene = (OdeScene) ps; diff --git a/OpenSim/Region/Physics/POSPlugin/POSScene.cs b/OpenSim/Region/Physics/POSPlugin/POSScene.cs index c3f5040..2f24a50 100644 --- a/OpenSim/Region/Physics/POSPlugin/POSScene.cs +++ b/OpenSim/Region/Physics/POSPlugin/POSScene.cs @@ -91,13 +91,7 @@ namespace OpenSim.Region.Physics.POSPlugin */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { POSPrim prim = new POSPrim(); prim.Position = position; diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs index 4de4b01..beb3404 100644 --- a/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs +++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs @@ -108,13 +108,7 @@ namespace OpenSim.Region.Physics.PhysXPlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) //To be removed - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { return AddPrim(position, size, rotation); } -- cgit v1.1 From 8dff9d564dfbf4841f03a919df44e8ae3d76de25 Mon Sep 17 00:00:00 2001 From: Melanie Date: Fri, 15 Jul 2011 19:35:49 +0200 Subject: Revert "Add localid support to ch0de properly" This reverts commit bb402d0d95c934fbcd8b1c03e228ec1d0a14f14d. --- .../Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs | 8 +++++++- .../Physics/BulletDotNETPlugin/BulletDotNETScene.cs | 7 ++++++- OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs | 8 +++++++- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 3 +-- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 14 ++++++++++---- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 19 ++++++++++++------- OpenSim/Region/Physics/OdePlugin/OdeScene.cs | 8 +++++++- .../Region/Physics/OdePlugin/Tests/ODETestClass.cs | 2 +- OpenSim/Region/Physics/POSPlugin/POSScene.cs | 8 +++++++- OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs | 8 +++++++- 10 files changed, 65 insertions(+), 20 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs index 6c9d9ab..b6e1cb4 100644 --- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs +++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs @@ -84,7 +84,13 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + Vector3 size, Quaternion rotation) + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) { return null; } diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs index 0d1bd82..6df213d 100644 --- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs @@ -213,7 +213,12 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin return newPrim; } - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation) + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical) { PhysicsActor result; IMesh mesh = null; diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs index 8e9edac..f4245b6 100644 --- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs +++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs @@ -626,7 +626,13 @@ namespace OpenSim.Region.Physics.BulletXPlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position, - OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, bool isPhysical, uint localid) + OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation) + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position, + OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, bool isPhysical) { PhysicsActor result; diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 82f1b94..7cd2dd1 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -256,9 +256,8 @@ namespace OpenSim.Region.Physics.OdePlugin public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) + Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) { - m_localID = localid; ode = dode; if (!pos.IsFinite()) { diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 353db44..a622745 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1717,7 +1717,7 @@ namespace OpenSim.Region.Physics.OdePlugin } private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, uint localid) + IMesh mesh, PrimitiveBaseShape pbs, bool isphysical) { Vector3 pos = position; @@ -1727,7 +1727,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode, localid); + newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode); lock (_prims) _prims.Add(newPrim); @@ -1749,7 +1749,13 @@ namespace OpenSim.Region.Physics.OdePlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + Vector3 size, Quaternion rotation) //To be removed + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) { PhysicsActor result; IMesh mesh = null; @@ -1757,7 +1763,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (needsMeshing(pbs)) mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical); - result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, localid); + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical); return result; } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index de22fae..13ea084 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -77,16 +77,15 @@ namespace OpenSim.Region.Physics.Manager public abstract void RemovePrim(PhysicsActor prim); - //public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - // Vector3 size, Quaternion rotation); //To be removed - Actually removed! - public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid); + Vector3 size, Quaternion rotation); //To be removed + public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical); public virtual PhysicsActor AddPrimShape(uint localID, string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + Vector3 size, Quaternion rotation, bool isPhysical) { - PhysicsActor ret = AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid); + PhysicsActor ret = AddPrimShape(primName, pbs, position, size, rotation, isPhysical); if (ret != null) ret.LocalID = localID; @@ -263,7 +262,13 @@ namespace OpenSim.Region.Physics.Manager */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + Vector3 size, Quaternion rotation) //To be removed + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) { m_log.InfoFormat("[PHYSICS]: NullPhysicsScene : AddPrim({0},{1})", position, size); return PhysicsActor.Null; diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs index 88902b0..f5172aa 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs @@ -1708,7 +1708,13 @@ namespace OpenSim.Region.Physics.OdePlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + Vector3 size, Quaternion rotation) //To be removed + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) { #if SPAM m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); diff --git a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs index 5dcd6f5..a7f8baa 100644 --- a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs +++ b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs @@ -83,7 +83,7 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f); Vector3 size = new Vector3(0.5f, 0.5f, 0.5f); Quaternion rot = Quaternion.Identity; - PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true, 0); + PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true); OdePrim oprim = (OdePrim)prim; OdeScene pscene = (OdeScene) ps; diff --git a/OpenSim/Region/Physics/POSPlugin/POSScene.cs b/OpenSim/Region/Physics/POSPlugin/POSScene.cs index 2f24a50..c3f5040 100644 --- a/OpenSim/Region/Physics/POSPlugin/POSScene.cs +++ b/OpenSim/Region/Physics/POSPlugin/POSScene.cs @@ -91,7 +91,13 @@ namespace OpenSim.Region.Physics.POSPlugin */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + Vector3 size, Quaternion rotation) + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) { POSPrim prim = new POSPrim(); prim.Position = position; diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs index beb3404..4de4b01 100644 --- a/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs +++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs @@ -108,7 +108,13 @@ namespace OpenSim.Region.Physics.PhysXPlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + Vector3 size, Quaternion rotation) //To be removed + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) { return AddPrim(position, size, rotation); } -- cgit v1.1 From c7dbd7cbd035ccc412624cd221348479d43798d6 Mon Sep 17 00:00:00 2001 From: Melanie Date: Fri, 15 Jul 2011 12:08:40 -0700 Subject: Fox some local id issues in physics glue --- .../Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs | 8 +------- .../Physics/BulletDotNETPlugin/BulletDotNETScene.cs | 7 +------ OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs | 8 +------- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 3 ++- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 14 ++++---------- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 19 +++++++------------ OpenSim/Region/Physics/OdePlugin/OdeScene.cs | 8 +------- .../Region/Physics/OdePlugin/Tests/ODETestClass.cs | 2 +- OpenSim/Region/Physics/POSPlugin/POSScene.cs | 8 +------- OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs | 8 +------- 10 files changed, 20 insertions(+), 65 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs index b6e1cb4..6c9d9ab 100644 --- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs +++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs @@ -84,13 +84,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { return null; } diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs index 6df213d..0d1bd82 100644 --- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs @@ -213,12 +213,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin return newPrim; } - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation) - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical) + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { PhysicsActor result; IMesh mesh = null; diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs index f4245b6..8e9edac 100644 --- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs +++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs @@ -626,13 +626,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position, - OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation) - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position, - OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, bool isPhysical) + OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, bool isPhysical, uint localid) { PhysicsActor result; diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 7cd2dd1..82f1b94 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -256,8 +256,9 @@ namespace OpenSim.Region.Physics.OdePlugin public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) + Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) { + m_localID = localid; ode = dode; if (!pos.IsFinite()) { diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index a622745..353db44 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1717,7 +1717,7 @@ namespace OpenSim.Region.Physics.OdePlugin } private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - IMesh mesh, PrimitiveBaseShape pbs, bool isphysical) + IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, uint localid) { Vector3 pos = position; @@ -1727,7 +1727,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode); + newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode, localid); lock (_prims) _prims.Add(newPrim); @@ -1749,13 +1749,7 @@ namespace OpenSim.Region.Physics.OdePlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) //To be removed - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { PhysicsActor result; IMesh mesh = null; @@ -1763,7 +1757,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (needsMeshing(pbs)) mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical); - result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical); + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, localid); return result; } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 13ea084..de22fae 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -77,15 +77,16 @@ namespace OpenSim.Region.Physics.Manager public abstract void RemovePrim(PhysicsActor prim); + //public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + // Vector3 size, Quaternion rotation); //To be removed - Actually removed! + public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation); //To be removed - public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical); + Vector3 size, Quaternion rotation, bool isPhysical, uint localid); public virtual PhysicsActor AddPrimShape(uint localID, string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { - PhysicsActor ret = AddPrimShape(primName, pbs, position, size, rotation, isPhysical); + PhysicsActor ret = AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid); if (ret != null) ret.LocalID = localID; @@ -262,13 +263,7 @@ namespace OpenSim.Region.Physics.Manager */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) //To be removed - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { m_log.InfoFormat("[PHYSICS]: NullPhysicsScene : AddPrim({0},{1})", position, size); return PhysicsActor.Null; diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs index f5172aa..88902b0 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs @@ -1708,13 +1708,7 @@ namespace OpenSim.Region.Physics.OdePlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) //To be removed - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { #if SPAM m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); diff --git a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs index a7f8baa..5dcd6f5 100644 --- a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs +++ b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs @@ -83,7 +83,7 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f); Vector3 size = new Vector3(0.5f, 0.5f, 0.5f); Quaternion rot = Quaternion.Identity; - PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true); + PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true, 0); OdePrim oprim = (OdePrim)prim; OdeScene pscene = (OdeScene) ps; diff --git a/OpenSim/Region/Physics/POSPlugin/POSScene.cs b/OpenSim/Region/Physics/POSPlugin/POSScene.cs index c3f5040..2f24a50 100644 --- a/OpenSim/Region/Physics/POSPlugin/POSScene.cs +++ b/OpenSim/Region/Physics/POSPlugin/POSScene.cs @@ -91,13 +91,7 @@ namespace OpenSim.Region.Physics.POSPlugin */ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { POSPrim prim = new POSPrim(); prim.Position = position; diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs index 4de4b01..beb3404 100644 --- a/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs +++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXScene.cs @@ -108,13 +108,7 @@ namespace OpenSim.Region.Physics.PhysXPlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation) //To be removed - { - return AddPrimShape(primName, pbs, position, size, rotation, false); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical) + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { return AddPrim(position, size, rotation); } -- cgit v1.1 From f2855d36686a20f2a40f138c9eb7f92d2b56b6c6 Mon Sep 17 00:00:00 2001 From: Melanie Date: Sat, 24 Dec 2011 01:41:32 +0100 Subject: Add setter for Acceleration on physics objects. --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 1 + OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 1 + 2 files changed, 2 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 58e2e4c..2945199 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -841,6 +841,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override Vector3 Acceleration { get { return _acceleration; } + set { _acceleration = value; } } public void SetAcceleration(Vector3 accel) diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 5f802d0..42e22ff 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -673,6 +673,7 @@ namespace OpenSim.Region.Physics.OdePlugin } return _acceleration; } + set { _acceleration = value; } } -- cgit v1.1 From 815f3af1d7b3bf16e81dd3a03e0c69c8e49f2f91 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 8 Feb 2012 15:24:10 +0000 Subject: UbitODE plugin initial commit --- .../Region/Physics/UbitOdePlugin/AssemblyInfo.cs | 58 + .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 1301 ++++++++ .../Physics/UbitOdePlugin/ODEDynamics.c_comments | 630 ++++ .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 1035 +++++++ OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 3260 ++++++++++++++++++++ .../UbitOdePlugin/ODERayCastRequestManager.cs | 443 +++ OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 1903 ++++++++++++ OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs | 86 + OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2741 ++++++++++++++++ OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs | 99 + 10 files changed, 11556 insertions(+) create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs new file mode 100644 index 0000000..d46341b --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs @@ -0,0 +1,58 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Reflection; +using System.Runtime.InteropServices; + +// Information about this assembly is defined by the following +// attributes. +// +// change them to the information which is associated with the assembly +// you compile. + +[assembly : AssemblyTitle("OdePlugin")] +[assembly : AssemblyDescription("Ubit Variation")] +[assembly : AssemblyConfiguration("")] +[assembly : AssemblyCompany("http://opensimulator.org")] +[assembly : AssemblyProduct("OdePlugin")] +[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")] +[assembly : AssemblyTrademark("")] +[assembly : AssemblyCulture("")] + +// This sets the default COM visibility of types in the assembly to invisible. +// If you need to expose a type to COM, use [ComVisible(true)] on that type. + +[assembly : ComVisible(false)] + +// The assembly version has following format : +// +// Major.Minor.Build.Revision +// +// You can specify all values by your own or you can build default build and revision +// numbers with the '*' character (the default): + +[assembly : AssemblyVersion("0.6.5.*")] diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs new file mode 100644 index 0000000..c8f7c76 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -0,0 +1,1301 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + + +// Revision by Ubit 2011 + +using System; +using System.Collections.Generic; +using System.Reflection; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using log4net; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. + /// + + public enum dParam : int + { + LowStop = 0, + HiStop = 1, + Vel = 2, + FMax = 3, + FudgeFactor = 4, + Bounce = 5, + CFM = 6, + StopERP = 7, + StopCFM = 8, + LoStop2 = 256, + HiStop2 = 257, + Vel2 = 258, + FMax2 = 259, + StopERP2 = 7 + 256, + StopCFM2 = 8 + 256, + LoStop3 = 512, + HiStop3 = 513, + Vel3 = 514, + FMax3 = 515, + StopERP3 = 7 + 512, + StopCFM3 = 8 + 512 + } + + public class OdeCharacter : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private Vector3 _position; + private Vector3 _zeroPosition; + private bool _zeroFlag = false; + private Vector3 _velocity; + private Vector3 _target_velocity; + private Vector3 _acceleration; + private Vector3 m_rotationalVelocity; + private float m_mass = 80f; + public float m_density = 60f; + private bool m_pidControllerActive = true; + public float PID_D = 800.0f; + public float PID_P = 900.0f; + //private static float POSTURE_SERVO = 10000.0f; + public float CAPSULE_RADIUS = 0.37f; + public float CAPSULE_LENGTH = 2.140599f; + public float walkDivisor = 1.3f; + public float runDivisor = 0.8f; + private bool flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_iscollidingObj = false; + private bool m_alwaysRun = false; + private int m_requestedUpdateFrequency = 0; + private Vector3 m_taintPosition = Vector3.Zero; + private bool m_hasTaintPosition = false; + public uint m_localID = 0; + public bool m_returnCollisions = false; + // taints and their non-tainted counterparts + public bool m_isPhysical = false; // the current physical status + public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing) + public float MinimumGroundFlightOffset = 3f; + + + private Vector3 m_taintForce; + private bool m_hasTaintForce; + private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. + + + private float m_buoyancy = 0f; + + // private CollisionLocker ode; + + private string m_name = String.Empty; + // other filter control + int m_colliderfilter = 0; + // int m_colliderGroundfilter = 0; + int m_colliderObjectfilter = 0; + + // Default we're a Character + private CollisionCategories m_collisionCategories = (CollisionCategories.Character); + + // Default, Collide with Other Geometries, spaces, bodies and characters. + private CollisionCategories m_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + // we do land collisions not ode | CollisionCategories.Land); + public IntPtr Body = IntPtr.Zero; + private OdeScene _parent_scene; + public IntPtr Shell = IntPtr.Zero; + public IntPtr Amotor = IntPtr.Zero; + public d.Mass ShellMass; + public bool collidelock = false; + + private bool m_haseventsubscription = false; + public int m_eventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + // unique UUID of this character object + public UUID m_uuid; + public bool bad = false; + + public ContactData AvatarContactData = new ContactData(10f, 0.3f); + + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) + { + m_uuid = UUID.Random(); + + m_hasTaintPosition = false; + + if (pos.IsFinite()) + { + if (pos.Z > 9999999f) + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (pos.Z < -90000f) + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + _position = pos; + } + else + { + _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); + m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); + } + + _parent_scene = parent_scene; + + PID_D = pid_d; + PID_P = pid_p; + CAPSULE_RADIUS = capsule_radius; + m_density = density; + m_mass = 80f; // sure we have a default + + AvatarContactData.mu = parent_scene.AvatarFriction; + AvatarContactData.bounce = parent_scene.AvatarBounce; + + walkDivisor = walk_divisor; + runDivisor = rundivisor; + + CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH; + + m_isPhysical = false; // current status: no ODE information exists + m_tainted_isPhysical = true; // new tainted status: need to create ODE information + + m_hasTaintForce = false; + _parent_scene.AddPhysicsActorTaint(this); + + m_name = avName; + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + + public override ContactData ContactData + { + get { return AvatarContactData; } + } + + public override bool Building { get; set; } + + /// + /// If this is set, the avatar will move faster + /// + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + + public override uint LocalID + { + set { m_localID = value; } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set { return; } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override bool FloatOnWater + { + set { return; } + } + + public override bool IsPhysical + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + + public override bool Flying + { + get { return flying; } + set + { + flying = value; + // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); + } + } + + /// + /// Returns if the avatar is colliding in general. + /// This includes the ground and objects and avatar. + /// + public override bool IsColliding + { + get { return (m_iscolliding || m_iscollidingGround); } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + { +// SetPidStatus(false); + m_pidControllerActive = true; + m_iscolliding = true; + } + } + } + + /// + /// Returns if an avatar is colliding with the ground + /// + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { + /* we now control this + if (value) + { + m_colliderGroundfilter += 2; + if (m_colliderGroundfilter > 2) + m_colliderGroundfilter = 2; + } + else + { + m_colliderGroundfilter--; + if (m_colliderGroundfilter < 0) + m_colliderGroundfilter = 0; + } + + if (m_colliderGroundfilter == 0) + m_iscollidingGround = false; + else + m_iscollidingGround = true; + */ + } + + } + + /// + /// Returns if the avatar is colliding with an object + /// + public override bool CollidingObj + { + get { return m_iscollidingObj; } + set + { + // Ubit filter this also + if (value) + { + m_colliderObjectfilter += 2; + if (m_colliderObjectfilter > 2) + m_colliderObjectfilter = 2; + } + else + { + m_colliderObjectfilter--; + if (m_colliderObjectfilter < 0) + m_colliderObjectfilter = 0; + } + + if (m_colliderObjectfilter == 0) + m_iscollidingObj = false; + else + m_iscollidingObj = true; + + // m_iscollidingObj = value; +/* + if (m_iscollidingObj) + m_pidControllerActive = false; + else + m_pidControllerActive = true; + */ + } + } + + /// + /// turn the PID controller on or off. + /// The PID Controller will turn on all by itself in many situations + /// + /// + public void SetPidStatus(bool status) + { + m_pidControllerActive = status; + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + /// + /// This 'puts' an avatar somewhere in the physics space. + /// Not really a good choice unless you 'know' it's a good + /// spot otherwise you're likely to orbit the avatar. + /// + public override Vector3 Position + { + get { return _position; } + set + { + if (Body == IntPtr.Zero || Shell == IntPtr.Zero) + { + if (value.IsFinite()) + { + if (value.Z > 9999999f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (value.Z < -90000f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + + m_taintPosition.X = value.X; + m_taintPosition.Y = value.Y; + m_taintPosition.Z = value.Z; + m_hasTaintPosition = true; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); + } + } + } + } + + public override Vector3 RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + + /// + /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight + /// and use it to offset landings properly + /// + public override Vector3 Size + { + get { + float d = CAPSULE_RADIUS * 2; + return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); } + set + { + if (value.IsFinite()) + { + m_pidControllerActive = true; + + Vector3 SetSize = value; + m_tainted_CAPSULE_LENGTH = SetSize.Z *1.15f - CAPSULE_RADIUS * 2.0f; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); + } + } + } + + /// + /// This creates the Avatar's physical Surrogate at the position supplied + /// + /// + /// + /// + + // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access + // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only + // place that is safe to call this routine AvatarGeomAndBodyCreation. + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) + { + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + if (CAPSULE_LENGTH <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_LENGTH = 0.01f; + + } + + if (CAPSULE_RADIUS <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_RADIUS = 0.01f; + + } + Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); + + d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); + d.GeomSetCollideBits(Shell, (int)m_collisionFlags); + + d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); + + m_mass = ShellMass.mass; // update mass + + // rescale PID parameters + PID_D = _parent_scene.avPIDD; + PID_P = _parent_scene.avPIDP; + + // rescale PID parameters so that this aren't affected by mass + // and so don't get unstable for some masses + // also scale by ode time step so you don't need to refix them + + PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps + PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; + PID_P /= 50 * 80; + PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; + + Body = d.BodyCreate(_parent_scene.world); + + d.BodySetAutoDisableFlag(Body, false); + d.BodySetPosition(Body, npositionX, npositionY, npositionZ); + + _position.X = npositionX; + _position.Y = npositionY; + _position.Z = npositionZ; + + m_hasTaintPosition = false; + + d.BodySetMass(Body, ref ShellMass); + d.GeomSetBody(Shell, Body); + + // The purpose of the AMotor here is to keep the avatar's physical + // surrogate from rotating while moving + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + d.JointSetAMotorNumAxes(Amotor, 3); + d.JointSetAMotorAxis(Amotor, 0, 0 , 1, 0, 0); + d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); + d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); + + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorAngle(Amotor, 1, 0); + d.JointSetAMotorAngle(Amotor, 2, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); + + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f); + } + + /// + /// Destroys the avatar body and geom + + private void AvatarGeomAndBodyDestroy() + { + // Kill the Amotor + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + Body = IntPtr.Zero; + } + + //kill the Geometry + if (Shell != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(Shell); + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } + } + // + /// + /// Uses the capped cyllinder volume formula to calculate the avatar's mass. + /// This may be used in calculations in the scene/scenepresence + /// + public override float Mass + { + get + { + float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH)); + return m_density * AVvolume; + } + } + public override void link(PhysicsActor obj) + { + + } + + public override void delink() + { + + } + + public override void LockAngularMotion(Vector3 axis) + { + + } + + + public override Vector3 Force + { + get { return _target_velocity; } + set { return; } + } + + public override int VehicleType + { + get { return 0; } + set { return; } + } + + public override void VehicleFloatParam(int param, float value) + { + + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + + } + + public override void VehicleFlags(int param, bool remove) + { + + } + + public override void SetVolumeDetect(int param) + { + + } + + public override Vector3 CenterOfMass + { + get + { + Vector3 pos = _position; + return pos; + } + } + + public override Vector3 GeometricCenter + { + get + { + Vector3 pos = _position; + return pos; + } + } + + //UBit mess + /* for later use + public override Vector3 PrimOOBsize + { + get + { + Vector3 s=Size; + s.X *=0.5f; + s.Y *=0.5f; + s.Z *=0.5f; + return s; + } + } + + public override Vector3 PrimOOBoffset + { + get + { + return Vector3.Zero; + } + } + */ + + public override PrimitiveBaseShape Shape + { + set { return; } + } + + public override Vector3 Velocity + { + get + { + return _velocity; + } + set + { + if (value.IsFinite()) + { + m_pidControllerActive = true; + _target_velocity = value; + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); + } + } + } + + public override Vector3 Torque + { + get { return Vector3.Zero; } + set { return; } + } + + public override float CollisionScore + { + get { return 0f; } + set { } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return Quaternion.Identity; } + set + { + //Matrix3 or = Orientation.ToRotationMatrix(); + //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22); + //d.BodySetRotation(Body, ref ord); + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public void SetAcceleration(Vector3 accel) + { + m_pidControllerActive = true; + _acceleration = accel; + } + + /// + /// Adds the force supplied to the Target Velocity + /// The PID controller takes this target velocity and tries to make it a reality + /// + /// + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + if (pushforce) + { + m_pidControllerActive = false; + m_taintForce = force / _parent_scene.ODE_STEPSIZE; + m_hasTaintForce = true; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_pidControllerActive = true; + _target_velocity.X += force.X; + _target_velocity.Y += force.Y; + _target_velocity.Z += force.Z; + } + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character"); + } + //m_lastUpdateSent = false; + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + + } + + public override void SetMomentum(Vector3 momentum) + { + } + + + /// + /// Called from Simulate + /// This is the avatar's movement control + PID Controller + /// + /// + public void Move(float timeStep, List defects) + { + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if (Body == IntPtr.Zero) + return; + + d.Vector3 dtmp; + d.BodyCopyPosition(Body, out dtmp); + Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + + // the Amotor still lets avatar rotation to drift during colisions + // so force it back to identity + + d.Quaternion qtmp; + qtmp.W = 1; + qtmp.X = 0; + qtmp.Y = 0; + qtmp.Z = 0; + d.BodySetQuaternion(Body, ref qtmp); + + if (m_pidControllerActive == false) + { + _zeroPosition = localpos; + } + //PidStatus = true; + + + if (!localpos.IsFinite()) + { + + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + + return; + } + + Vector3 vec = Vector3.Zero; + dtmp = d.BodyGetLinearVel(Body); + Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + + float movementdivisor = 1f; + //Ubit change divisions into multiplications below + if (!m_alwaysRun) + { + movementdivisor = 1 / walkDivisor; + } + else + { + movementdivisor = 1 / runDivisor; + } + + // colide with land + + d.AABB aabb; + d.GeomGetAABB(Shell, out aabb); + float chrminZ = aabb.MinZ; + + Vector3 posch = localpos; + + float ftmp; + + if (flying) + { + ftmp = timeStep; + posch.X += vel.X * ftmp; + posch.Y += vel.Y * ftmp; + } + + float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); + if (chrminZ < terrainheight) + { + float depth = terrainheight - chrminZ; + if (!flying) + { + vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50; + } + else + vec.Z = depth * PID_P * 50; + + /* + Vector3 vtmp; + vtmp.X = _target_velocity.X * timeStep; + vtmp.Y = _target_velocity.Y * timeStep; + // fake and avoid squares + float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y)); + if (k > 0) + { + posch.X += vtmp.X; + posch.Y += vtmp.Y; + terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); + k = 1 + Math.Abs(terrainheight) / k; + movementdivisor /= k; + + if (k < 1) + k = 1; + } + */ + + + if (depth < 0.1f) + { + m_iscolliding = true; + m_colliderfilter = 2; + m_iscollidingGround = true; + + ContactPoint contact = new ContactPoint(); + contact.PenetrationDepth = depth; + contact.Position.X = localpos.X; + contact.Position.Y = localpos.Y; + contact.Position.Z = chrminZ; + contact.SurfaceNormal.X = 0f; + contact.SurfaceNormal.Y = 0f; + contact.SurfaceNormal.Z = -1f; + AddCollisionEvent(0, contact); + + vec.Z *= 0.5f; + } + + else + m_iscollidingGround = false; + } + else + m_iscollidingGround = false; + + + // if velocity is zero, use position control; otherwise, velocity control + if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f + && m_iscolliding) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!_zeroFlag) + { + _zeroFlag = true; + _zeroPosition = localpos; + } + if (m_pidControllerActive) + { + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2); + vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2); + if (flying) + { + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + } + } + //PidStatus = true; + } + else + { + m_pidControllerActive = true; + _zeroFlag = false; + + if (m_iscolliding) + { + if (!flying) + { + if (_target_velocity.Z > 0.0f) + { + // We're colliding with something and we're not flying but we're moving + // This means we're walking or running. JUMPING + vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; + } + // We're standing on something + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D); + } + else + { + // We're flying and colliding with something + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f); + vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + } + } + else // ie not colliding + { + if (flying) //(!m_iscolliding && flying) + { + // we're in mid air suspended + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f); + vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + } + + else + { + // we're not colliding and we're not flying so that means we're falling! + // m_iscolliding includes collisions with the ground. + + // d.Vector3 pos = d.BodyGetPosition(Body); + vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f; + vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f; + } + } + } + + if (flying) + { + vec.Z -= _parent_scene.gravityz * m_mass; + + //Added for auto fly height. Kitto Flora + float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset; + + if (localpos.Z < target_altitude) + { + vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; + } + // end add Kitto Flora + } + + if (vec.IsFinite()) + { + if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) + d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + } + } + + /// + /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. + /// + public void UpdatePositionAndVelocity() + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (Body == IntPtr.Zero) + return; + + d.Vector3 vec; + try + { + d.BodyCopyPosition(Body, out vec); + } + catch (NullReferenceException) + { + bad = true; + _parent_scene.BadCharacter(this); + vec = new d.Vector3(_position.X, _position.Y, _position.Z); + base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem! + m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid); + } + + _position.X = vec.X; + _position.Y = vec.Y; + _position.Z = vec.Z; + + bool fixbody = false; + + if (_position.X < 0.0f) + { + fixbody = true; + _position.X = 0.1f; + } + else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f) + { + fixbody = true; + _position.X = (int)_parent_scene.WorldExtents.X - 0.1f; + } + + if (_position.Y < 0.0f) + { + fixbody = true; + _position.Y = 0.1f; + } + else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1) + { + fixbody = true; + _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f; + } + + if (fixbody) + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + // Did we move last? = zeroflag + // This helps keep us from sliding all over +/* + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + // Did we send out the 'stopped' message? + if (!m_lastUpdateSent) + { + m_lastUpdateSent = true; + base.RequestPhysicsterseUpdate(); + } + } + else + { + m_lastUpdateSent = false; + */ + try + { + vec = d.BodyGetLinearVel(Body); + } + catch (NullReferenceException) + { + vec.X = _velocity.X; + vec.Y = _velocity.Y; + vec.Z = _velocity.Z; + } + _velocity.X = (vec.X); + _velocity.Y = (vec.Y); + _velocity.Z = (vec.Z); + // } + } + + /// + /// Cleanup the things we use in the scene. + /// + public void Destroy() + { + m_tainted_isPhysical = false; + _parent_scene.AddPhysicsActorTaint(this); + } + + public override void CrossingFailure() + { + } + + public override Vector3 PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } + + public override float PIDHoverHeight { set { return; } } + public override bool PIDHoverActive { set { return; } } + public override PIDHoverType PIDHoverType { set { return; } } + public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + + public override void SubscribeEvents(int ms) + { + m_requestedUpdateFrequency = ms; + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + m_haseventsubscription = true; + } + + public override void UnSubscribeEvents() + { + m_haseventsubscription = false; + _parent_scene.RemoveCollisionEventReporting(this); + m_requestedUpdateFrequency = 0; + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (m_haseventsubscription) + { + // m_log.DebugFormat( + // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + } + + public void SendCollisions() + { + if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency) + { + if (CollisionEventsThisFrame != null) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + } + CollisionEventsThisFrame = new CollisionEventUpdate(); + m_eventsubscription = 0; + } + } + + public override bool SubscribedEvents() + { + return m_haseventsubscription; + } + + public void ProcessTaints(float timestep) + { + + if (m_tainted_isPhysical != m_isPhysical) + { + if (m_tainted_isPhysical) + { + // Create avatar capsule and related ODE data + if ((Shell != IntPtr.Zero)) + { + // a lost shell ? + m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " + + (Shell != IntPtr.Zero ? "Shell " : "") + + (Body != IntPtr.Zero ? "Body " : "") + + (Amotor != IntPtr.Zero ? "Amotor " : "")); + AvatarGeomAndBodyDestroy(); + } + + AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + _parent_scene.AddCharacter(this); + } + else + { + _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + } + + m_isPhysical = m_tainted_isPhysical; + } + + if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) + { + if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + { + AvatarGeomAndBodyDestroy(); + + m_pidControllerActive = true; + + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; + + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); + + Velocity = Vector3.Zero; + + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell == IntPtr.Zero ? "Shell " : "") + + (Body == IntPtr.Zero ? "Body " : "") + + (Amotor == IntPtr.Zero ? "Amotor " : "")); + } + } + + if (m_hasTaintPosition) + { + if (Body != IntPtr.Zero) + d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); + + _position.X = m_taintPosition.X; + _position.Y = m_taintPosition.Y; + _position.Z = m_taintPosition.Z; + m_hasTaintPosition = false; + } + + if (m_hasTaintForce) + { + if (Body != IntPtr.Zero) + { + if(m_taintForce.X !=0f || m_taintForce.Y !=0f || m_taintForce.Z !=0) + d.BodyAddForce(Body, m_taintForce.X, m_taintForce.Y, m_taintForce.Z); + m_hasTaintForce = false; + } + } + + } + + internal void AddCollisionFrameTime(int p) + { + // protect it from overflow crashing + if (m_eventsubscription + p >= int.MaxValue) + m_eventsubscription = 0; + m_eventsubscription += p; + } + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments new file mode 100644 index 0000000..1060aa6 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments @@ -0,0 +1,630 @@ +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + * + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + + public IntPtr Body + { + get { return m_body; } + } + + private int frcount = 0; // Used to limit dynamics debug output to + // every 100th frame + + // private OdeScene m_parentScene = null; + private IntPtr m_body = IntPtr.Zero; + private IntPtr m_jointGroup = IntPtr.Zero; + private IntPtr m_aMotor = IntPtr.Zero; + + + // Vehicle properties + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + private Vector3 m_linearFrictionTimescale = Vector3.Zero; + private float m_linearMotorDecayTimescale = 0; + private float m_linearMotorTimescale = 0; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + // private bool m_LinearMotorSetLastFrame = false; + // private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; + private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; + private Vector3 m_angularFrictionTimescale = Vector3.Zero; + private float m_angularMotorDecayTimescale = 0; + private float m_angularMotorTimescale = 0; + private Vector3 m_lastAngularVelocityVector = Vector3.Zero; + + //Deflection properties + // private float m_angularDeflectionEfficiency = 0; + // private float m_angularDeflectionTimescale = 0; + // private float m_linearDeflectionEfficiency = 0; + // private float m_linearDeflectionTimescale = 0; + + //Banking properties + // private float m_bankingEfficiency = 0; + // private float m_bankingMix = 0; + // private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 0f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 0; + private float m_verticalAttractionTimescale = 0; + + + + + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0.0f) pValue = 0.0f; + if (pValue > 1.0f) pValue = 1.0f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + break; + + } + + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + // m_referenceFrame = pValue; + break; + } + + }//end ProcessRotationVehicleParam + + internal void ProcessTypeChange(Vehicle pType) + { +Console.WriteLine("ProcessTypeChange to " + pType); + + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 1; + // m_linearDeflectionTimescale = 1; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 1; + // m_bankingTimescale = 10; + // m_referenceFrame = Quaternion.Identity; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // // m_linearDeflectionEfficiency = 1; + // // m_linearDeflectionTimescale = 2; + // // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 10; + // m_bankingEfficiency = -0.2f; + // m_bankingMix = 1; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10,10,10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 0.5f; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5; + // m_bankingEfficiency = -0.3f; + // m_bankingMix = 0.8f; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2; + // m_bankingEfficiency = 1; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 2; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0; + // m_linearDeflectionTimescale = 5; + // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 5; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + + } + }//end SetDefaultsForType + + internal void Enable(IntPtr pBody, OdeScene pParentScene) + { +//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy); + if (m_type == Vehicle.TYPE_NONE) + return; + + m_body = pBody; + //KF: This used to set up the linear and angular joints + } + + internal void Step(float pTimestep, OdeScene pParentScene) + { + if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) + return; + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; + + MoveLinear(pTimestep, pParentScene); + MoveAngular(pTimestep); + }// end Step + + private void MoveLinear(float pTimestep, OdeScene _pParentScene) + { + if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + + // add drive to body + Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); + m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? + + // This will work temporarily, but we really need to compare speed on an axis + // KF: Limit body velocity to applied velocity? + if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) + m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; + if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) + m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) + m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; + + // decay applied velocity + Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_linearMotorDirection -= m_linearMotorDirection * decayfraction; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + else + { // requested is not significant + // if what remains of applied is small, zero it. + if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) + m_lastLinearVelocityVector = Vector3.Zero; + } + + + // convert requested object velocity to world-referenced vector + m_dir = m_lastLinearVelocityVector; + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + m_dir *= rotq; // apply obj rotation to velocity vector + + // add Gravity andBuoyancy + // KF: So far I have found no good method to combine a script-requested + // .Z velocity and gravity. Therefore only 0g will used script-requested + // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. + Vector3 grav = Vector3.Zero; + if(m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); + // Preserve the current Z velocity + d.Vector3 vel_now = d.BodyGetLinearVel(Body); + m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity + } // else its 1.0, no gravity. + + // Check if hovering + if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + +// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped +// m_VhoverTimescale = 0f; // time to acheive height +// pTimestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; +//if(frcount == 0) Console.WriteLine("herr0=" + herr0); + // Replace Vertical speed with correction figure if significant + if(Math.Abs(herr0) > 0.01f ) + { + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); + // m_VhoverEfficiency is not yet implemented + } + else + { + m_dir.Z = 0f; + } + } + + // Apply velocity + d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); +//if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); +//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z); + + + // apply friction + Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); + m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; + } // end MoveLinear() + + private void MoveAngular(float pTimestep) + { + + // m_angularMotorDirection is the latest value from the script, and is decayed here + // m_angularMotorDirectionLASTSET is the latest value from the script + // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here + + if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + // ramp up to new value + Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); + m_lastAngularVelocityVector += (addAmount * 10f); +//if(frcount == 0) Console.WriteLine("add: " + addAmount); + + // limit applied value to what was set by script + // This will work temporarily, but we really need to compare speed on an axis + if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X)) + m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; + if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y)) + m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) + m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; + + // decay the requested value + Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_angularMotorDirection -= m_angularMotorDirection * decayfraction; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ? + + // Vertical attractor section + +// d.Mass objMass; +// d.BodyGetMass(Body, out objMass); +// float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); + float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); + // get present body rotation + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by servo + verterr = verterr * servo; + + // rotate to object frame + // verterr = verterr * rotq; + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + m_lastAngularVelocityVector.X += verterr.Y; + m_lastAngularVelocityVector.Y -= verterr.X; +/* +if(frcount == 0) + { +// Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector); + Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}", + Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency)); + } + */ + d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z); + // apply friction + Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); + m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; + + } //end MoveAngular + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs new file mode 100644 index 0000000..363cbef --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -0,0 +1,1035 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + +// private OdeScene m_parentScene = null; +// private IntPtr m_aMotor = IntPtr.Zero; + + + private OdePrim rootPrim; + private OdeScene _pParentScene; + + + + + private Vector3 refUpAxis = new Vector3(0, 0, 1); + private Vector3 refAtAxis = new Vector3(1, 0, 0); + + + // Vehicle properties + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + + private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl + private Quaternion m_RollreferenceFrame = Quaternion.Identity; + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + private float m_linearMotorDecayTimescale = 120; + private float m_linearMotorTimescale = 1000; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + + //Deflection properties + private float m_angularDeflectionEfficiency = 0; + private float m_angularDeflectionTimescale = 1000; + private float m_linearDeflectionEfficiency = 0; + private float m_linearDeflectionTimescale = 1000; + + //Banking properties + private float m_bankingEfficiency = 0; + private float m_bankingMix = 0; + private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 1000f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped + private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. + + // special contact data for vehicles + public ContactData VehiculeContactData = new ContactData(0f, 0.1f); + + // auxiliar + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + + private float m_lmEfect = 0; // current linear motor eficiency + private float m_amEfect = 0; // current angular motor eficiency + + + public ODEDynamics(OdePrim rootp) + { + rootPrim = rootp; + _pParentScene = rootPrim._parent_scene; + } + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + float len; + float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE; + float timestep = _pParentScene.ODE_STEPSIZE; + + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + m_angularMotorDecayTimescale = pValue * invtimestep; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + m_linearMotorDecayTimescale = pValue * invtimestep; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + len = m_angularMotorDirection.Length(); + if (len > 12.566f) + m_angularMotorDirection *= (12.566f / len); + m_amEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + len = m_linearMotorDirection.Length(); + if (len > 30.0f) + m_linearMotorDirection *= (30.0f / len); + m_lmEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + len = m_linearMotorOffset.Length(); + if (len > 100.0f) + m_linearMotorOffset *= (100.0f / len); + break; + } + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + float len; + float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE; + float timestep = _pParentScene.ODE_STEPSIZE; + switch (pParam) + { + + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + len = m_angularMotorDirection.Length(); + if (len > 12.566f) + m_angularMotorDirection *= (12.566f / len); + m_amEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = m_linearMotorDirection.Length(); + if (len > 30.0f) + m_linearMotorDirection *= (30.0f / len); + m_lmEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = m_linearMotorOffset.Length(); + if (len > 100.0f) + m_linearMotorOffset *= (100.0f / len); + break; + case Vehicle.BLOCK_EXIT: + m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + m_referenceFrame = Quaternion.Inverse(pValue); + break; + case Vehicle.ROLL_FRAME: + m_RollreferenceFrame = pValue; + break; + } + }//end ProcessRotationVehicleParam + + internal void ProcessVehicleFlags(int pParam, bool remove) + { + if (remove) + { + m_flags &= ~((VehicleFlag)pParam); + } + else + { + m_flags |= (VehicleFlag)pParam; + } + }//end ProcessVehicleFlags + + internal void ProcessTypeChange(Vehicle pType) + { + float invtimestep = _pParentScene.ODE_STEPSIZE; + m_lmEfect = 0; + m_amEfect = 0; + + m_linearMotorDirection = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + + m_BlockingEndPoint = Vector3.Zero; + m_RollreferenceFrame = Quaternion.Identity; + m_linearMotorOffset = Vector3.Zero; + + m_referenceFrame = Quaternion.Identity; + + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_NONE: + m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120 * invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 1000 * invtimestep; + m_VhoverHeight = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_flags = (VehicleFlag)0; + break; + + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120 * invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120 * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 1; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 1000; + m_bankingEfficiency = 0; + m_bankingMix = 1; + m_bankingTimescale = 10; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 2; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 10f; + m_bankingEfficiency = -0.2f; + m_bankingMix = 1; + m_bankingTimescale = 1; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4 * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 3; + m_angularDeflectionEfficiency = 0.5f; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5f; + m_bankingEfficiency = -0.3f; + m_bankingMix = 0.8f; + m_bankingTimescale = 1; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_ROLL_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_WATER_ONLY); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 8 * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 0.5f; + m_angularDeflectionEfficiency = 1; + m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + m_bankingEfficiency = 1; + m_bankingMix = 0.7f; + m_bankingTimescale = 2; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10 * invtimestep; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0; + m_linearDeflectionTimescale = 5 * invtimestep; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0f; + m_verticalAttractionTimescale = 1000f; + m_bankingEfficiency = 0; + m_bankingMix = 0.7f; + m_bankingTimescale = 5; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + } + + }//end SetDefaultsForType + + internal void Stop() + { + m_lmEfect = 0; + m_amEfect = 0; + } + + public static Vector3 Xrot(Quaternion rot) + { + Vector3 vec; + rot.Normalize(); // just in case + vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1; + vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W); + vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W); + return vec; + } + + public static Vector3 Zrot(Quaternion rot) + { + Vector3 vec; + rot.Normalize(); // just in case + vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W); + vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W); + vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1; + + return vec; + } + + private const float halfpi = 0.5f * (float)Math.PI; + + public static Vector3 ubitRot2Euler(Quaternion rot) + { + // returns roll in X + // pitch in Y + // yaw in Z + Vector3 vec; + + // assuming rot is normalised + // rot.Normalize(); + + float zX = rot.X * rot.Z + rot.Y * rot.W; + + if (zX < -0.49999f) + { + vec.X = 0; + vec.Y = -halfpi; + vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W)); + } + else if (zX > 0.49999f) + { + vec.X = 0; + vec.Y = halfpi; + vec.Z = (float)(2d * Math.Atan(rot.X / rot.W)); + } + else + { + vec.Y = (float)Math.Asin(2 * zX); + + float sqw = rot.W * rot.W; + + float minuszY = rot.X * rot.W - rot.Y * rot.Z; + float zZ = rot.Z * rot.Z + sqw - 0.5f; + + vec.X = (float)Math.Atan2(minuszY, zZ); + + float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?) + float yY = rot.X * rot.X + sqw - 0.5f; + vec.Z = (float)Math.Atan2(yX, yY); + } + return vec; + } + + + public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) + { + // assuming rot is normalised + // rot.Normalize(); + + float zX = rot.X * rot.Z + rot.Y * rot.W; + + if (zX < -0.49999f) + { + roll = 0; + pitch = -halfpi; + } + else if (zX > 0.49999f) + { + roll = 0; + pitch = halfpi; + } + else + { + pitch = (float)Math.Asin(2 * zX); + + float minuszY = rot.X * rot.W - rot.Y * rot.Z; + float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f; + + roll = (float)Math.Atan2(minuszY, zZ); + } + return ; + } + + internal void Step()//float pTimestep) + { + IntPtr Body = rootPrim.Body; + + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion rotq = objrotq; // rotq = rotation of object + rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame + Quaternion irotq = Quaternion.Inverse(rotq); + + d.Vector3 dvtmp; + Vector3 tmpV; + dvtmp = d.BodyGetLinearVel(Body); + Vector3 curVel; + curVel.X = dvtmp.X; + curVel.Y = dvtmp.Y; + curVel.Z = dvtmp.Z; + Vector3 curLocalVel = curVel * irotq; // current velocity in local + + dvtmp = d.BodyGetAngularVel(Body); + Vector3 curAngVel; + curAngVel.X = dvtmp.X; + curAngVel.Y = dvtmp.Y; + curAngVel.Z = dvtmp.Z; + Vector3 curLocalAngVel = curAngVel * irotq; // current velocity in local + + Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame + Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in object frame + d.Vector3 dtorque = new d.Vector3();// actually angular aceleration until mult by Inertia in object frame + + bool doathing = false; + + // linear motor + if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) + { + tmpV = m_linearMotorDirection - curLocalVel; // velocity error + if (tmpV.LengthSquared() > 1e-6f) + { + tmpV = tmpV * (m_lmEfect / m_linearMotorTimescale); // error to correct in this timestep + tmpV *= rotq; // to world + + if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) + tmpV.Z = 0; + + if (m_linearMotorOffset.X != 0 && m_linearMotorOffset.Y != 0 && m_linearMotorOffset.Z != 0) + { + // have offset, do it now + tmpV *= rootPrim.Mass; + d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); + } + else + { + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + } + m_lmEfect *= (1 - 1.0f / m_linearMotorDecayTimescale); + } + else + m_lmEfect = 0; + + // friction + if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) + { + tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; + tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; + tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; + tmpV *= rotq; // to world + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + + // hover + if (m_VhoverTimescale < 300) + { + d.Vector3 pos = d.BodyGetPosition(Body); + + // default to global + float perr = m_VhoverHeight - pos.Z;; + + if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) + { + perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + } + else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) + { + perr += _pParentScene.GetWaterLevel(); + } + else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) + { + float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + float w = _pParentScene.GetWaterLevel(); + if (t > w) + perr += t; + else + perr += w; + } + + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) + { + force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE; + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + } + else // no buoyancy + force.Z += _pParentScene.gravityz; + } + else + { + // default gravity and buoancy + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + } + + // linear deflection + if (m_linearDeflectionEfficiency > 0) + { + float len = curVel.Length(); + Vector3 atAxis = refAtAxis; + atAxis *= rotq; // at axis rotated to world + atAxis = Xrot(rotq); + tmpV = atAxis * len; + tmpV -= curVel; // velocity error + tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep + force.X += tmpV.X; + force.Y += tmpV.Y; + if((m_flags & VehicleFlag.NO_DEFLECTION_UP) ==0) + force.Z += tmpV.Z; + } + + // angular motor + if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) + { + tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error + if (tmpV.LengthSquared() > 1e-6f) + { + tmpV = tmpV * (m_amEfect / m_angularMotorTimescale); // error to correct in this timestep + tmpV *= m_referenceFrame; // to object + dtorque.X += tmpV.X; + dtorque.Y += tmpV.Y; + dtorque.Z += tmpV.Z; + } + m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale); + } + else + m_amEfect = 0; + + // angular friction + if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) + { + tmpV.X = -curLocalAngVel.X / m_angularFrictionTimescale.X; + tmpV.Y = -curLocalAngVel.Y / m_angularFrictionTimescale.Y; + tmpV.Z = -curLocalAngVel.Z / m_angularFrictionTimescale.Z; + tmpV *= m_referenceFrame; // to object + dtorque.X += tmpV.X; + dtorque.Y += tmpV.Y; + dtorque.Z += tmpV.Z; + } + + // angular deflection + if (m_angularDeflectionEfficiency > 0) + { + doathing = false; + float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale / m_angularDeflectionTimescale /_pParentScene.ODE_STEPSIZE; + tmpV.X = 0; + if (Math.Abs(curLocalVel.Z) > 0.01) + { + tmpV.Y = -(float)Math.Atan2(curLocalVel.Z, curLocalVel.X) * ftmp; + doathing = true; + } + else + tmpV.Y = 0; + if (Math.Abs(curLocalVel.Y) > 0.01) + { + tmpV.Z = (float)Math.Atan2(curLocalVel.Y, curLocalVel.X) * ftmp; + doathing = true; + } + else + tmpV.Z = 0; + + if (doathing) + { + tmpV *= m_referenceFrame; // to object + dtorque.X += tmpV.X; + dtorque.Y += tmpV.Y; + dtorque.Z += tmpV.Z; + } + } + + // vertical atractor + if (m_verticalAttractionTimescale < 300) + { + doathing = false; + float roll; + float pitch; + + GetRollPitch(rotq, out roll, out pitch); + + + float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; + float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; + + if (Math.Abs(roll) > 0.01) // roll + { + tmpV.X = -roll * ftmp; + tmpV.X -= curLocalAngVel.X * ftmp2; + doathing = true; + } + else + { + tmpV.X = 0; + } + + if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch + { + tmpV.Y = -pitch * ftmp; + tmpV.Y -= curLocalAngVel.Y * ftmp2; + doathing = true; + } + else + { + tmpV.Y = 0; + } + + tmpV.Z = 0; + + if (m_bankingEfficiency == 0 || Math.Abs(roll) < 0.01) + tmpV.Z = 0; + else + { + float broll = -roll * m_bankingEfficiency; ; + if (m_bankingMix != 0) + { + float vfact = m_bankingMix * Math.Abs(curLocalVel.X) / 10.0f; + if (vfact < m_bankingMix) + broll *= ((1 - m_bankingMix) + vfact); + } + + tmpV.Z = (broll - curLocalAngVel.Z) / m_bankingTimescale; + doathing = true; + } + + if (doathing) + { + + tmpV *= m_referenceFrame; // to object + dtorque.X += tmpV.X; + dtorque.Y += tmpV.Y; + dtorque.Z += tmpV.Z; + } + } + /* + d.Vector3 pos = d.BodyGetPosition(Body); + // Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f); + Vector3 posChange = new Vector3(); + posChange.X = pos.X - m_lastPositionVector.X; + posChange.Y = pos.Y - m_lastPositionVector.Y; + posChange.Z = pos.Z - m_lastPositionVector.Z; + double Zchange = Math.Abs(posChange.Z); + if (m_BlockingEndPoint != Vector3.Zero) + { + if (pos.X >= (m_BlockingEndPoint.X - (float)1)) + { + pos.X -= posChange.X + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + if (pos.Y >= (m_BlockingEndPoint.Y - (float)1)) + { + pos.Y -= posChange.Y + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + if (pos.Z >= (m_BlockingEndPoint.Z - (float)1)) + { + pos.Z -= posChange.Z + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + if (pos.X <= 0) + { + pos.X += posChange.X + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + if (pos.Y <= 0) + { + pos.Y += posChange.Y + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + } + if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y)) + { + pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + + } + if ((m_flags & (VehicleFlag.NO_X)) != 0) + { + m_dir.X = 0; + } + if ((m_flags & (VehicleFlag.NO_Y)) != 0) + { + m_dir.Y = 0; + } + if ((m_flags & (VehicleFlag.NO_Z)) != 0) + { + m_dir.Z = 0; + } + + + */ + // angular part + /* + + // Get what the body is doing, this includes 'external' influences +/* + Vector3 angularVelocity = Vector3.Zero; + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if (m_verticalAttractionTimescale < 300) + { + float VAservo = 0.2f / m_verticalAttractionTimescale; + // get present body rotation + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by VAservo + verterr = verterr * VAservo; +//if (frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = - verterr.X; + vertattr.Z = 0f; + + // scaling appears better usingsquare-law + float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); + vertattr.X += bounce * angularVelocity.X; + vertattr.Y += bounce * angularVelocity.Y; + + } // else vertical attractor is off + + // m_lastVertAttractor = vertattr; + + // Bank section tba + // Deflection section tba + + // Sum velocities + m_lastAngularVelocity = angularVelocity + vertattr; // + bank + deflection + + if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) + { + m_lastAngularVelocity.X = 0; + m_lastAngularVelocity.Y = 0; + } + + if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) + { + if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + } + else + { + m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. + } +*/ + + d.Mass dmass; + d.BodyGetMass(Body,out dmass); + + if (force.X != 0 || force.Y != 0 || force.Z != 0) + { + force *= dmass.mass; + d.BodySetForce(Body, force.X, force.Y, force.Z); + } + + if (dtorque.X != 0 || dtorque.Y != 0 || dtorque.Z != 0) + { + d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); + d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame + } + + //end MoveAngular + + // limit rotations +/* + bool changed = false; + + if (m_RollreferenceFrame != Quaternion.Identity) + { + if (rotq.X >= m_RollreferenceFrame.X) + { + rot.X = rotq.X - (m_RollreferenceFrame.X / 2); + } + if (rotq.Y >= m_RollreferenceFrame.Y) + { + rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2); + } + if (rotq.X <= -m_RollreferenceFrame.X) + { + rot.X = rotq.X + (m_RollreferenceFrame.X / 2); + } + if (rotq.Y <= -m_RollreferenceFrame.Y) + { + rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2); + } + changed = true; + } + + if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0) + { + rot.X = 0; + rot.Y = 0; + changed = true; + } + if (changed) + d.BodySetQuaternion(Body, ref rot); +*/ + } + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs new file mode 100644 index 0000000..1a53c99 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -0,0 +1,3260 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* Revision 2011 by Ubit Umarov + * + * + */ + +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_isphysical; + private bool m_fakeisphysical; + + protected bool m_building; + private Quaternion m_lastorientation = new Quaternion(); + private Quaternion _orientation; + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _torque; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 _acceleration; + private Vector3 m_angularlock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; + + private Vector3 m_force; + private Vector3 m_forceacc; + private Vector3 m_angularForceacc; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private float PID_D = 35f; + private float PID_G = 25f; + private bool m_usePID; + + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private float m_targetHoverHeight; + private float m_groundHeight; + private float m_waterHeight; + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + + private int body_autodisable_frames = 20; + + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + private bool m_collidesLand = true; + private bool m_collidesWater; + public bool m_returnCollisions; + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlags; + + public bool m_disabled; + public bool m_taintselected; + + public uint m_localID; + + private PrimitiveBaseShape _pbs; + public OdeScene _parent_scene; + + /// + /// The physics space which contains prim geometry + /// + public IntPtr m_targetSpace = IntPtr.Zero; + + public IntPtr prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent; + + private List childrenPrim = new List(); + + private bool m_iscolliding; + private bool m_wascolliding; + private bool m_isSelected; + + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + private bool m_throttleUpdates; + private int throttleCounter; + public int m_interpenetrationcount; + public float m_collisionscore; + int m_colliderfilter = 0; + public int m_roundsUnderMotionThreshold; + private int m_crossingfailures; + + public bool outofBounds; + private float m_density = 10.000006836f; // Aluminum g/cm3; + + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body = IntPtr.Zero; + public String Name { get; private set; } + private Vector3 _target_velocity; + + public Vector3 primOOBsize; // prim real dimensions from mesh + public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb + public float primOOBradiusSQ; + public d.Mass primdMass; // prim inertia information on it's own referencial + float primMass; // prim own mass + float _mass; // object mass acording to case + public d.Mass objectpMass; // object last computed inertia + private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb + + public int givefakepos = 0; + private Vector3 fakepos; + public int givefakeori = 0; + private Quaternion fakeori; + + public int m_eventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + private IntPtr m_linkJoint = IntPtr.Zero; + private IntPtr _linkJointGroup = IntPtr.Zero; + + public volatile bool childPrim; + + public ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f}; + + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical // this is not reliable for internal use + { + get { return m_fakeisphysical; } + set + { + m_fakeisphysical = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + if (!value) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + AddChange(changes.Physical, value); + } + } + + public override bool Building // this is not reliable for internal use + { + get { return m_building; } + set + { + if (value) + m_building = true; + AddChange(changes.building, value); + } + } + + public override ContactData ContactData + { + get + { +/* + ODEDynamics v; + if(childPrim && _parent !=null) + { + v =((OdePrim)_parent).m_vehicle; + if(v != null && v.Type != Vehicle.TYPE_NONE) + return v.VehiculeContactData; + return primContactData; + } + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + return m_vehicle.VehiculeContactData; +*/ + return primContactData; + } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Prim; } + set { return; } + } + + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + + public override uint LocalID + { + get + { + return m_localID; + } + set + { + //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); + m_localID = value; + } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set + { + if(value) + m_isSelected = value; + AddChange(changes.Selected, value); + } + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + m_iscolliding = true; + + if (m_wascolliding != m_iscolliding) + { + if (m_wascolliding && !m_isSelected && Body != IntPtr.Zero) + d.BodyEnable(Body); + m_wascolliding = m_iscolliding; + } + } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get + { + if (givefakepos > 0) + return fakepos; + else + return _position; + } + + set + { + fakepos = value; + givefakepos++; + AddChange(changes.Position, value); + } + } + + public override Vector3 Size + { + get { return _size; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Size, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + } + } + } + + public override float Mass + { + get { return _mass; } + } + + public override Vector3 Force + { + //get { return Vector3.Zero; } + get { return m_force; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Force, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + } + } + } + + + public override void SetVolumeDetect(int param) + { + AddChange(changes.VolumeDtc,(param != 0)); + } + + public override Vector3 GeometricCenter + { + get + { + return Vector3.Zero; + } + } + + public override Vector3 CenterOfMass + { + get + { + d.Vector3 dtmp; + if (IsPhysical && !childPrim && Body != IntPtr.Zero) + { + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; + + Vector3 vtmp = primOOBoffset * q; + dtmp = d.GeomGetPosition(prim_geom); + return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); + } + else + return Vector3.Zero; + } + } + /* + public override Vector3 PrimOOBsize + { + get + { + return primOOBsize; + } + } + + public override Vector3 PrimOOBoffset + { + get + { + return primOOBoffset; + } + } + + public override float PrimOOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } + } + */ + public override PrimitiveBaseShape Shape + { + set + { + AddChange(changes.Shape, value); + } + } + + public override Vector3 Velocity + { + get + { + // Averate previous velocity with the new one so + // client object interpolation works a 'little' better + if (_zeroFlag) + return Vector3.Zero; +/* + Vector3 returnVelocity = Vector3.Zero; + returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; + + return returnVelocity; + */ + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); +// _velocity = value; + + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); + } + + } + } + + public override Vector3 Torque + { + get + { + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return _torque; + } + + set + { + if (value.IsFinite()) + { + AddChange(changes.Torque, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); + } + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get + { + if (givefakeori>0) + return fakeori; + else + + return _orientation; + } + set + { + if (QuaternionIsFinite(value)) + { + fakeori = value; + givefakeori++; + AddChange(changes.Orientation, value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); + + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + m_buoyancy = value; + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + public override int VehicleType + { + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + if (m_vehicle == null) + { + if (value != (int)Vehicle.TYPE_NONE) + { + m_vehicle = new ODEDynamics(this); + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + else + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + + public override void VehicleFloatParam(int param, float value) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessFloatVehicleParam((Vehicle)param, value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)param, value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + public override void VehicleFlags(int param, bool remove) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(param, remove); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + m_crossingfailures++; + changeDisable(false); + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + primContactData.mu = _parent_scene.m_materialContactsData[pMaterial].mu; + primContactData.bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public void setPrimForRemoval() + { + AddChange(changes.Remove, null); + } + + public override void link(PhysicsActor obj) + { + AddChange(changes.Link, obj); + } + + public override void delink() + { + AddChange(changes.DeLink, null); + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axis); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + _parent_scene.RemoveCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) + CollisionEventsThisFrame = null; + else + CollisionEventsThisFrame = new CollisionEventUpdate(); + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) + { + Name = primName; + + m_vehicle = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); + } + _position = pos; + givefakepos = 0; + + PID_D = parent_scene.bodyPIDD; + PID_G = parent_scene.bodyPIDG; + m_density = parent_scene.geomDefaultDensity; + // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + Body = IntPtr.Zero; + + if (!size.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + } + + _orientation = rotation; + givefakeori = 0; + + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_wascolliding = false; + m_colliderfilter = 0; + + hasOOBoffsetFromMesh = false; + _triMeshData = IntPtr.Zero; + + + primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + CalcPrimBodyData(); + + m_building = true; // control must set this to false when done + + AddChange(changes.Add, null); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_disabled = false; + } + + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); + + if (axisnum <= 0) + return; + + // stop it + d.BodySetTorque(Body, 0, 0, 0); + d.BodySetAngularVel(Body, 0, 0, 0); + + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + + d.JointSetAMotorNumAxes(Amotor, axisnum); + + // get current orientation to lock + + d.Quaternion dcur = d.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; + Vector3 ax; + + const int StopERP = 7; + const int StopCFM = 8; + + int i = 0; + int j = 0; + if (axis.X == 0) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + // ODE should do this with axis relative to body 1 but seems to fail + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)StopERP, 0.8f); + i++; + j = 256; // odeplugin.cs doesn't have all parameters so this moves to next axis set + } + + if (axis.Y == 0) + { + ax = (new Vector3(0, 1, 0)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f); + i++; + j += 256; + } + + if (axis.Z == 0) + { + ax = (new Vector3(0, 0, 1)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f); + } + } + + private bool setMesh(OdeScene parent_scene) + { + if (Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this,false); + } + } + else + { + DestroyBody(); + } + } + + IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true); + if (mesh == null) + { + m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); + return false; + } + + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z); + _size.X = 0.01f; + _size.Y = 0.01f; + _size.Z = 0.01f; + return false; + } + +// primOOBoffset = mesh.GetCentroid(); +// hasOOBoffsetFromMesh = true; + hasOOBoffsetFromMesh = false; + + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + mesh.releaseSourceMeshData(); + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + return false; + } + return true; + } + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + CalcPrimBodyData(); + + _parent_scene.geom_name_map[prim_geom] = Name; + _parent_scene.actor_name_map[prim_geom] = this; + +/* + if (childPrim) + { + if (_parent != null && _parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; + //Console.WriteLine("SetGeom calls ChildSetGeom"); + parent.ChildSetGeom(this); + } + } + */ + } + else + m_log.Warn("Setting bad Geom"); + } + + + /// + /// Create a geometry for the given mesh in the given target space. + /// + /// + /// If null, then a mesh is used that is based on the profile shape data. + private void CreateGeom() + { + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + + bool haveMesh = false; + hasOOBoffsetFromMesh = false; + + if (_parent_scene.needsMeshing(_pbs)) + { + haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + } + + if(!haveMesh) + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return; + } + } + else + {// do it as a box + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + //Console.WriteLine(" CreateGeom 4"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + } + } + + /// + /// Set a new geometry for this prim. + /// + /// + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(prim_geom); + _parent_scene.actor_name_map.Remove(prim_geom); + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + // catch (System.AccessViolationException) + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name,e); + } + + prim_geom = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + Body = IntPtr.Zero; + hasOOBoffsetFromMesh = false; +// CalcPrimBodyData(); + } + + private void ChildSetGeom(OdePrim odePrim) + { + // well.. + DestroyBody(); + MakeBody(); + } + + //sets non physical prim m_targetSpace to right space in spaces grid for static prims + // should only be called for non physical prims unless they are becoming non physical + private void SetInStaticSpace(OdePrim prim) + { + IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); + prim.m_targetSpace = targetSpace; + d.GeomEnable(prim_geom); + } + + public void enableBodySoft() + { + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) + { + /* + if (m_targetSpace != _parent_scene.ActiveSpace) + { + m_targetSpace = _parent_scene.ActiveSpace; + + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom != IntPtr.Zero) + { + d.SpaceAdd(m_targetSpace, prm.prim_geom); + prm.m_targetSpace = m_targetSpace; + } + } + d.SpaceAdd(m_targetSpace, prim_geom); + } + */ + d.GeomEnable(prim_geom); + foreach (OdePrim prm in childrenPrim) + d.GeomEnable(prm.prim_geom); + + d.BodyEnable(Body); + } + } + resetCollisionAccounting(); // this sets m_disable to false + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) + { + /* + if (m_targetSpace == _parent_scene.ActiveSpace) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.m_targetSpace != IntPtr.Zero && prm.prim_geom != IntPtr.Zero) + { + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + prm.m_targetSpace = IntPtr.Zero; + } + } + d.SpaceRemove(m_targetSpace, prim_geom); + m_targetSpace = IntPtr.Zero; + } + */ + d.GeomDisable(prim_geom); + foreach (OdePrim prm in childrenPrim) + d.GeomDisable(prm.prim_geom); + d.BodyDisable(Body); + } + } + } + + private void MakeBody() + { + if (!m_isphysical) // only physical get bodies + return; + + if (childPrim) // child prims don't get bodies; + return; + + if (m_building) + return; + + if (prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); + return; + } + + if (Body != IntPtr.Zero) + { + d.BodyDestroy(Body); + Body = IntPtr.Zero; + m_log.Warn("[PHYSICS]: MakeBody called having a body"); + } + + + if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); + } + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + Body = d.BodyCreate(_parent_scene.world); + + DMassDup(ref primdMass, out objdmass); + + // rotate inertia + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + // set the body rotation and position + d.BodySetRotation(Body, ref mymat); + + // recompute full object inertia if needed + if (childrenPrim.Count > 0) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; + Vector3 rcm; + + rcm.X = _position.X + objdmass.c.X; + rcm.Y = _position.Y + objdmass.c.Y; + rcm.Z = _position.Z + objdmass.c.Z; + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); + continue; + } + + + + DMassCopy(ref prm.primdMass, ref tmpdmass); + + // apply prim current rotation to inertia + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + quat.W = prm._orientation.W; + d.RfromQ(out mat, ref quat); + d.MassRotate(ref tmpdmass, ref mat); + + Vector3 ppos = prm._position; + ppos.X += tmpdmass.c.X - rcm.X; + ppos.Y += tmpdmass.c.Y - rcm.Y; + ppos.Z += tmpdmass.c.Z - rcm.Z; + + // refer inertia to root prim center of mass position + d.MassTranslate(ref tmpdmass, + ppos.X, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats + + if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); + } + + d.GeomClearOffset(prm.prim_geom); + d.GeomSetBody(prm.prim_geom, Body); + prm.Body = Body; + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.W = -myrot.W; + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + // d.BodySetLinearDampingThreshold(Body, 0.01f); + // d.BodySetAngularDampingThreshold(Body, 0.001f); + d.BodySetDamping(Body, .001f, .0002f); + + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + m_interpenetrationcount = 0; + m_collisionscore = 0; + + m_disabled = false; + + if (m_targetSpace != _parent_scene.ActiveSpace) + { + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } + + m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prim_geom); + } + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + continue; + + Vector3 ppos = prm._position; + d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position + + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + + if (prm.m_targetSpace != _parent_scene.ActiveSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } + d.GeomEnable(prm.prim_geom); + prm.m_disabled = false; + prm.m_interpenetrationcount = 0; + prm.m_collisionscore = 0; + _parent_scene.addActivePrim(prm); + } + } + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + + d.GeomEnable(prim_geom); + m_disabled = false; + _parent_scene.addActivePrim(this); + } + + private void DestroyBody() + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + + if (!childPrim) + { + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + prm.m_collisionCategories &= ~CollisionCategories.Body; + prm.m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prm.prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); + } + prm.Body = IntPtr.Zero; + prm._mass = prm.primMass; + prm.m_collisionscore = 0; + } + } + d.BodyDestroy(Body); + } + Body = IntPtr.Zero; + } + _mass = primMass; + m_disabled = true; + m_collisionscore = 0; + } + + #region Mass Calculation + + private float CalculatePrimVolume() + { + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.52359877559829887307710723054658f; + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + return volume; + } + + + private void CalcPrimBodyData() + { + float volume; + + if (prim_geom == IntPtr.Zero) + { + // Ubit let's have a initial basic OOB + primOOBsize.X = _size.X; + primOOBsize.Y = _size.Y; + primOOBsize.Z = _size.Z; + primOOBoffset = Vector3.Zero; + } + else + { + d.AABB AABB; + d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom + + primOOBsize.X = (AABB.MaxX - AABB.MinX); + primOOBsize.Y = (AABB.MaxY - AABB.MinY); + primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); + if (!hasOOBoffsetFromMesh) + { + primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; + primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; + primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; + } + } + + // also its own inertia and mass + // keep using basic shape mass for now + volume = CalculatePrimVolume(); + + primMass = m_density * volume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + _mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); + + d.MassTranslate(ref primdMass, + primOOBoffset.X, + primOOBoffset.Y, + primOOBoffset.Z); + + primOOBsize *= 0.5f; // let obb size be a corner coords + primOOBradiusSQ = primOOBsize.LengthSquared(); + } + + + #endregion + + + /// + /// Add a child prim to this parent prim. + /// + /// Child prim + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) + { + //Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) + { + DestroyBody(); // for now we need to rebuil entire object on link change + + lock (childrenPrim) + { + // adopt the prim + if (!childrenPrim.Contains(prim)) + childrenPrim.Add(prim); + + // see if this prim has kids and adopt them also + // should not happen for now + foreach (OdePrim prm in prim.childrenPrim) + { + if (!childrenPrim.Contains(prm)) + { + if (prm.Body != IntPtr.Zero) + { + if (prm.prim_geom != IntPtr.Zero) + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + if(prm.Body != prim.Body) + prm.DestroyBody(); // don't loose bodies around + prm.Body = IntPtr.Zero; + } + + childrenPrim.Add(prm); + prm._parent = this; + } + } + } + //Remove old children from the prim + prim.childrenPrim.Clear(); + + if (prim.Body != IntPtr.Zero) + { + if (prim.prim_geom != IntPtr.Zero) + d.GeomSetBody(prim.prim_geom, IntPtr.Zero); + prim.DestroyBody(); // don't loose bodies around + prim.Body = IntPtr.Zero; + } + + prim.childPrim = true; + prim._parent = this; + + MakeBody(); // full nasty reconstruction + } + } + + private void UpdateChildsfromgeom() + { + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.UpdateDataFromGeom(); + } + } + + private void UpdateDataFromGeom() + { + if (prim_geom != IntPtr.Zero) + { + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + d.Quaternion qtmp = new d.Quaternion { }; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.W = qtmp.W; + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + } + } + + private void ChildDelink(OdePrim odePrim, bool remakebodies) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) // delinking the root prim + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + if (remakebodies) + newroot.MakeBody(); + } + } + } + + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + // odePrim.UpdateDataFromGeom(); + if (remakebodies) + odePrim.MakeBody(); + } + } + if (remakebodies) + MakeBody(); + } + + protected void ChildRemove(OdePrim odePrim, bool reMakeBody) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + newroot.MakeBody(); + } + } + if (reMakeBody) + MakeBody(); + return; + } + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + if (reMakeBody) + odePrim.MakeBody(); + } + } + MakeBody(); + } + + #region changes + + private void changeadd() + { + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + // _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; + if (!m_isphysical) + SetInStaticSpace(this); + } + + m_building = false; // REMOVE THIS LATER + + + if (m_isphysical && Body == IntPtr.Zero) + { +/* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + { + */ + MakeBody(); +// } + } + } + + private void changeAngularLock(Vector3 newLock) + { + // do we have a Physical object? + if (Body != IntPtr.Zero) + { + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) + { + if (!newLock.ApproxEquals(Vector3.One, 0f)) + { + createAMotor(newLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = newLock; + } + + private void changeLink(OdePrim NewParent) + { + if (_parent == null && NewParent != null) + { + NewParent.ParentPrim(this); + } + else if (_parent != null) + { + if (_parent is OdePrim) + { + if (NewParent != _parent) + { + (_parent as OdePrim).ChildDelink(this,false); // for now... + childPrim = false; + + if (NewParent != null) + { + NewParent.ParentPrim(this); + } + } + } + } + _parent = NewParent; + } + + + private void Stop() + { + if(!childPrim) + { + m_force = Vector3.Zero; + m_forceacc = Vector3.Zero; + m_angularForceacc = Vector3.Zero; + _torque = Vector3.Zero; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + } + + if (Body != IntPtr.Zero) + { + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetTorque(Body, 0f, 0f, 0f); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetAngularVel(Body, 0f, 0f, 0f); + + } + } + + private void changeSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); + + if (newval) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + disableBodySoft(); + } + else + { + m_collisionCategories = CollisionCategories.Geom; + + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags; + + if (m_collidesLand) + m_collisionFlags |= CollisionCategories.Land; + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + enableBodySoft(); + } + + resetCollisionAccounting(); + } + + private void changePosition(Vector3 newPos) + { + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _position = newPos; + } + } + else + { + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; +// changeSelectedStatus(); + resetCollisionAccounting(); + } + + private void changeOrientation(Quaternion newOri) + { + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _orientation = newOri; + } + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + } + } + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + resetCollisionAccounting(); + } + + private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + { + if (m_isphysical) + { + if (childPrim && m_building) // inertia is messed, must rebuild + { + _position = newPos; + _orientation = newOri; + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + + resetCollisionAccounting(); + } + + + private void changeDisable(bool disable) + { + if (disable) + { + if (!m_disabled) + disableBodySoft(); + } + else + { + if (m_disabled) + enableBodySoft(); + } + } + + private void changePhysicsStatus(bool NewStatus) + { + m_isphysical = NewStatus; + + if (!childPrim) + { + if (NewStatus) + { + if (Body == IntPtr.Zero) + { +/* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ + { + MakeBody(); + } + } + } + else + { + if (Body != IntPtr.Zero) + { + // UpdateChildsfromgeom(); +/* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ + DestroyBody(); + } + } + } + + resetCollisionAccounting(); + } + + private void changeprimsizeshape() + { + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + // we don't need to do space calculation because the client sends a position update also. + if (_size.X <= 0) + _size.X = 0.01f; + if (_size.Y <= 0) + _size.Y = 0.01f; + if (_size.Z <= 0) + _size.Z = 0.01f; + // Construction of new prim + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + + resetCollisionAccounting(); + } + + private void changeSize(Vector3 newSize) + { + _size = newSize; + changeprimsizeshape(); + } + + private void changeShape(PrimitiveBaseShape newShape) + { + _pbs = newShape; + changeprimsizeshape(); + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; + + if (prim_geom != IntPtr.Zero) + { + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } + else + { + m_collisionFlags &= ~CollisionCategories.Water; + } + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } + + private void changeSetTorque(Vector3 newtorque) + { + if (!m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + } + _torque = newtorque; + } + } + + private void changeForce(Vector3 force) + { + m_force = force; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeAddForce(Vector3 force) + { + m_forceacc += force; + if (!m_isSelected) + { + lock (this) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + + m_collisionscore = 0; + m_interpenetrationcount = 0; + } + } + + private void changeAddAngularForce(Vector3 aforce) + { + m_angularForceacc += aforce; + if (!m_isSelected) + { + lock (this) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + m_collisionscore = 0; + m_interpenetrationcount = 0; + } + } + + private void changevelocity(Vector3 newVel) + { + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; + } + + private void changeVolumedetetion(bool newVolDtc) + { + m_isVolumeDetect = newVolDtc; + } + + protected void changeBuilding(bool newbuilding) + { + if ((bool)newbuilding) + { + m_building = true; + DestroyBody(); + } + else + { + m_building = false; + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } + } + + #endregion + + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building) // KF: Only move root prims. + { +// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + float timestep = _parent_scene.ODE_STEPSIZE; + + float fx = 0; + float fy = 0; + float fz = 0; + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + } + else + { + float m_mass = _mass; + + // fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + if (m_usePID) + { + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID) + { + //Console.WriteLine("Hover " + Name); + + // If we're using the PID controller, then we have no gravity + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } + else + { + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } + + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; + + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; + + m_forceacc = Vector3.Zero; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + + Vector3 trq; + + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } + + } + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; + //Console.WriteLine("Nothing " + Name); + + } + } + + + public void UpdatePositionAndVelocity(float simulatedtime) + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null && !m_disabled && !m_building) + { + if (Body != IntPtr.Zero) + { + if (m_crossingfailures != 0 && m_crossingfailures < 5) + { + _position.X = Util.Clip(_position.X, 0.4f, _parent_scene.WorldExtents.X - 0.4f); + _position.Y = Util.Clip(_position.Y, 0.4f, _parent_scene.WorldExtents.Y - 0.4f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + float tmp = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y); + if (_position.Z < tmp) + _position.Z = tmp + 0.2f; + + m_lastposition = _position; + m_lastorientation = _orientation; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + m_rotationalVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + m_crossingfailures = 0; // do this only once + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + enableBodySoft(); + base.RequestPhysicsterseUpdate(); + return; + } + + else if (m_crossingfailures != 0) + { + return; + } + + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom,out lpos); // root position that is seem by rest of simulator + + // we need to use root position since that's all the rest of scene uses + if ( lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X + || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y + ) + { + // we are outside current region + // we can't let it keeping moving and having colisions + // since it can be stucked between something like terrain and edge + // so lets stop and disable it until something else kicks it + if (m_crossingfailures == 0) + { + + _position.X = Util.Clip(lpos.X, -0.5f, _parent_scene.WorldExtents.X + 0.5f); + _position.Y = Util.Clip(lpos.Y, -0.5f, _parent_scene.WorldExtents.Y + 0.5f); + _position.Z = Util.Clip(lpos.Z, -100f, 50000f); + + m_lastposition = _position; + m_lastorientation = _orientation; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + m_crossingfailures++; // do this only once + base.RequestPhysicsterseUpdate(); + return; + } + } + + if (lpos.Z < -100 || lpos.Z > 100000f) + { + lpos.Z = Util.Clip(lpos.Z, -100f, 50000f); + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + + return; + } + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + + if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) + && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) + && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) + && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) + && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) + && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) + ) + { + _zeroFlag = true; + //Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } + + if (_zeroFlag) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastVelocity = _velocity; + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + + _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + { + m_rotationalVelocity = pv; + } + else + { + m_rotationalVelocity.X = rotvel.X; + m_rotationalVelocity.Y = rotvel.Y; + m_rotationalVelocity.Z = rotvel.Z; + } + + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + m_lastposition = _position; + m_lastorientation = _orientation; + base.RequestPhysicsterseUpdate(); + } + else + { + throttleCounter++; + } + } + } + else if (!m_lastUpdateSent || !_zeroFlag) + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + } + } + + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } + + internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) + { + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + + private static void DMassDup(ref d.Mass src, out d.Mass dst) + { + dst = new d.Mass { }; + + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; + case changes.Remove: + //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... + //When we return true, it destroys all of the prims in the linkset anyway + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildRemove(this,false); + } + else + ChildRemove(this,false); + + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + if (m_eventsubscription > 0) + UnSubscribeEvents(); + return true; + + case changes.Link: + OdePrim tmp = (OdePrim)arg; + changeLink(tmp); + break; + + case changes.DeLink: + changeLink(null); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changevelocity((Vector3)arg); + break; + +// case changes.Acceleration: +// changeacceleration((Vector3)arg); +// break; +// case changes.AngVelocity: +// changeangvelocity((Vector3)arg); +// break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.Shape: + changeShape((PrimitiveBaseShape) arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool) arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; + + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange(this, what, arg); + } + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs new file mode 100644 index 0000000..4b3f83b --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -0,0 +1,443 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenMetaverse; +using OpenSim.Region.Physics.Manager; +using OdeAPI; +using log4net; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Processes raycast requests as ODE is in a state to be able to do them. + /// This ensures that it's thread safe and there will be no conflicts. + /// Requests get returned by a different thread then they were requested by. + /// + public class ODERayCastRequestManager + { + /// + /// Pending ray requests + /// + protected OpenSim.Framework.LocklessQueue m_PendingRequests = new OpenSim.Framework.LocklessQueue(); + + /// + /// Scene that created this object. + /// + private OdeScene m_scene; + + IntPtr ray; + + private const int ColisionContactGeomsPerTest = 5; + + /// + /// ODE near callback delegate + /// + private d.NearCallback nearCallback; + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + private List m_contactResults = new List(); + + public ODERayCastRequestManager(OdeScene pScene) + { + m_scene = pScene; + nearCallback = near; + ray = d.CreateRay(IntPtr.Zero, 1.0f); + } + + /// + /// Queues a raycast + /// + /// Origin of Ray + /// Ray normal + /// Ray length + /// Return method to send the results + public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.Count = 0; + req.length = length; + req.Normal = direction; + req.Origin = position; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.Count = 0; + req.length = length; + req.Normal = direction; + req.Origin = position; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + + m_PendingRequests.Enqueue(req); + } + + /// + /// Queues a raycast + /// + /// Origin of Ray + /// Ray normal + /// Ray length + /// + /// Return method to send the results + public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + /// + /// Process all queued raycast requests + /// + /// Time in MS the raycasts took to process. + public int ProcessQueuedRequests() + { + int time = System.Environment.TickCount; + + if (m_PendingRequests.Count <= 0) + return 0; + + if (m_scene.ContactgeomsArray == IntPtr.Zero) // oops something got wrong or scene isn't ready still + { + m_PendingRequests.Clear(); + return 0; + } + + ODERayRequest req; + + int i = 50; // arbitary limit of processed tests per frame + + while(m_PendingRequests.Dequeue(out req)) + { + if (req.geom == IntPtr.Zero) + doSpaceRay(req); + else + doGeomRay(req); + if(--i < 0) + break; + } + + lock (m_contactResults) + m_contactResults.Clear(); + + return System.Environment.TickCount - time; + } + /// + /// Method that actually initiates the raycast with full top space + /// + /// + private void doSpaceRay(ODERayRequest req) + { + // Create the ray +// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length); + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + + // Collide test + d.SpaceCollide2(m_scene.TopSpace, ray, IntPtr.Zero, nearCallback); + + // Remove Ray +// d.GeomDestroy(ray); + + if (req.callbackMethod == null) + return; + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = 999999999999f; + Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + hitYN = true; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + ((RayCallback)req.callbackMethod)(m_contactResults); + lock (m_PendingRequests) + m_contactResults.Clear(); + } + } + + /// + /// Method that actually initiates the raycast with a geom + /// + /// + private void doGeomRay(ODERayRequest req) + { + // Create the ray +// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length); + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + + // Collide test + d.SpaceCollide2(req.geom, ray, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test + + // Remove Ray +// d.GeomDestroy(ray); + + if (req.callbackMethod == null) + return; + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = 999999999999f; + Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + hitYN = true; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + ((RayCallback)req.callbackMethod)(m_contactResults); + lock (m_PendingRequests) + m_contactResults.Clear(); + } + } + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; + if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest) + return false; + + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; + } + + // This is the standard Near. g2 is the ray + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + //Don't test against heightfield Geom, or you'll be sorry! + // Exclude heightfield geom + + if (g1 == IntPtr.Zero || g1 == g2) + return; + + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + return; + + // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms. + if (d.GeomIsSpace(g1)) + { + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); + } + return; + } + + int count = 0; + try + { + count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (SEHException) + { + m_log.Error("[PHYSICS Ray]: 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."); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); + return; + } + + if (count == 0) + return; + + PhysicsActor p1 = null; + + if (g1 != IntPtr.Zero) + m_scene.actor_name_map.TryGetValue(g1, out p1); + + d.ContactGeom curcontact = new d.ContactGeom(); + // Loop over contacts, build results. + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + if (p1 != null) { + if (p1 is OdePrim) + { + ContactResult collisionresult = new ContactResult(); + + collisionresult.ConsumerID = ((OdePrim)p1).m_localID; + collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Depth = curcontact.depth; + collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, + curcontact.normal.Z); + lock (m_contactResults) + m_contactResults.Add(collisionresult); + } + } + } + } + + /// + /// Dereference the creator scene so that it can be garbage collected if needed. + /// + internal void Dispose() + { + m_scene = null; + } + } + + public struct ODERayRequest + { + public IntPtr geom; + public Vector3 Origin; + public Vector3 Normal; + public int Count; + public float length; + public object callbackMethod; + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs new file mode 100644 index 0000000..c0c7ff3 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -0,0 +1,1903 @@ +/* + * based on: + * Ode.NET - .NET bindings for ODE + * Jason Perkins (starkos@industriousone.com) + * Licensed under the New BSD + * Part of the OpenDynamicsEngine +Open Dynamics Engine +Copyright (c) 2001-2007, Russell L. Smith. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +Neither the names of ODE's copyright owner nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * changes by opensim team; + * changes by Aurora team http://www.aurora-sim.org/ + + * Revision/fixs by Ubit Umarov + */ + +using System; +using System.Runtime.InteropServices; +using System.Security; + +namespace OdeAPI +{ +//#if dDOUBLE +// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim +// at least we save same memory and memory access time, FPU performance on intel usually is similar +// using dReal = System.Double; +//#else + using dReal = System.Single; +//#endif + + public static class d + { + public static dReal Infinity = dReal.MaxValue; + public static int NTotalBodies = 0; + + #region Flags and Enumerations + + [Flags] + public enum AllocateODEDataFlags : uint + { + BasicData = 0, + CollisionData = 0x00000001, + All = ~0u + } + + [Flags] + public enum IniteODEFlags : uint + { + dInitFlagManualThreadCleanup = 0x00000001 + } + + [Flags] + public enum ContactFlags : int + { + Mu2 = 0x001, + FDir1 = 0x002, + Bounce = 0x004, + SoftERP = 0x008, + SoftCFM = 0x010, + Motion1 = 0x020, + Motion2 = 0x040, + MotionN = 0x080, + Slip1 = 0x100, + Slip2 = 0x200, + Approx0 = 0x0000, + Approx1_1 = 0x1000, + Approx1_2 = 0x2000, + Approx1 = 0x3000 + } + + public enum GeomClassID : int + { + SphereClass, + BoxClass, + CapsuleClass, + CylinderClass, + PlaneClass, + RayClass, + ConvexClass, + GeomTransformClass, + TriMeshClass, + HeightfieldClass, + FirstSpaceClass, + SimpleSpaceClass = FirstSpaceClass, + HashSpaceClass, + QuadTreeSpaceClass, + LastSpaceClass = QuadTreeSpaceClass, + FirstUserClass, + LastUserClass = FirstUserClass + MaxUserClasses - 1, + NumClasses, + MaxUserClasses = 4 + } + + public enum JointType : int + { + None, + Ball, + Hinge, + Slider, + Contact, + Universal, + Hinge2, + Fixed, + Null, + AMotor, + LMotor, + Plane2D + } + + public enum JointParam : int + { + LoStop, + HiStop, + Vel, + FMax, + FudgeFactor, + Bounce, + CFM, + StopERP, + StopCFM, + SuspensionERP, + SuspensionCFM, + LoStop2 = 256, + HiStop2, + Vel2, + FMax2, + FudgeFactor2, + Bounce2, + CFM2, + StopERP2, + StopCFM2, + SuspensionERP2, + SuspensionCFM2, + LoStop3 = 512, + HiStop3, + Vel3, + FMax3, + FudgeFactor3, + Bounce3, + CFM3, + StopERP3, + StopCFM3, + SuspensionERP3, + SuspensionCFM3 + } + + public enum dSweepAndPruneAxis : int + { + XYZ = ((0)|(1<<2)|(2<<4)), + XZY = ((0)|(2<<2)|(1<<4)), + YXZ = ((1)|(0<<2)|(2<<4)), + YZX = ((1)|(2<<2)|(0<<4)), + ZXY = ((2)|(0<<2)|(1<<4)), + ZYX = ((2)|(1<<2)|(0<<4)) + } + + #endregion + + #region Callbacks + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GetAABBFn(IntPtr geom, out AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate ColliderFn GetColliderFnFn(int num); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GeomDtorFn(IntPtr o); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v); + + #endregion + + #region Structs + + [StructLayout(LayoutKind.Sequential)] + public struct AABB + { + public dReal MinX, MaxX; + public dReal MinY, MaxY; + public dReal MinZ, MaxZ; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Contact + { + public SurfaceParameters surface; + public ContactGeom geom; + public Vector3 fdir1; + public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact)); + } + + + [StructLayout(LayoutKind.Sequential)] + public struct ContactGeom + { + + public Vector3 pos; + public Vector3 normal; + public dReal depth; + public IntPtr g1; + public IntPtr g2; + public int side1; + public int side2; + public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom)); + } + + [StructLayout(LayoutKind.Sequential)] + public struct GeomClass + { + public int bytes; + public GetColliderFnFn collider; + public GetAABBFn aabb; + public AABBTestFn aabb_test; + public GeomDtorFn dtor; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct JointFeedback + { + public Vector3 f1; + public Vector3 t1; + public Vector3 f2; + public Vector3 t2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Mass + { + public dReal mass; + public Vector4 c; + public Matrix3 I; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix3 + { + public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22) + { + M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f; + M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f; + M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f; + } + public dReal M00, M10, M20; + private dReal _m30; + public dReal M01, M11, M21; + private dReal _m31; + public dReal M02, M12, M22; + private dReal _m32; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4 + { + public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30, + dReal m01, dReal m11, dReal m21, dReal m31, + dReal m02, dReal m12, dReal m22, dReal m32, + dReal m03, dReal m13, dReal m23, dReal m33) + { + M00 = m00; M10 = m10; M20 = m20; M30 = m30; + M01 = m01; M11 = m11; M21 = m21; M31 = m31; + M02 = m02; M12 = m12; M22 = m22; M32 = m32; + M03 = m03; M13 = m13; M23 = m23; M33 = m33; + } + public dReal M00, M10, M20, M30; + public dReal M01, M11, M21, M31; + public dReal M02, M12, M22, M32; + public dReal M03, M13, M23, M33; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public dReal W, X, Y, Z; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct SurfaceParameters + { + public ContactFlags mode; + public dReal mu; + public dReal mu2; + public dReal bounce; + public dReal bounce_vel; + public dReal soft_erp; + public dReal soft_cfm; + public dReal motion1; + public dReal motion2; + public dReal motionN; + public dReal slip1; + public dReal slip2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public Vector3(dReal x, dReal y, dReal z) + { + X = x; Y = y; Z = z; _w = 0.0f; + } + public dReal X, Y, Z; + private dReal _w; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector4 + { + public Vector4(dReal x, dReal y, dReal z, dReal w) + { + X = x; Y = y; Z = z; W = w; + } + public dReal X, Y, Z, W; + } + + #endregion + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity] + public static extern int AllocateODEDataForThread(uint ODEInitFlags); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnected(IntPtr b1, IntPtr b2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyiCreate(IntPtr world); + public static IntPtr BodyCreate(IntPtr world) + { + NTotalBodies++; + return BodyiCreate(world); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void BodyiDestroy(IntPtr body); + public static void BodyDestroy(IntPtr body) + { + NTotalBodies--; + BodyiDestroy(body); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyDisable(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyEnable(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetAutoDisableFlag(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetAutoDisableSteps(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableTime(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body); + public static Vector3 BodyGetAngularVel(IntPtr body) + { + unsafe { return *(BodyGetAngularVelUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetData(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetFiniteRotationMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body); + public static Vector3 BodyGetForce(IntPtr body) + { + unsafe { return *(BodyGetForceUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetGravityMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetGyroscopicMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetJoint(IntPtr body, int index); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body); + public static Vector3 BodyGetLinearVel(IntPtr body) + { + unsafe { return *(BodyGetLinearVelUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetMass(IntPtr body, out Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetNumJoints(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body); + public static Vector3 BodyGetPosition(IntPtr body) + { + unsafe { return *(BodyGetPositionUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body); + public static Quaternion BodyGetQuaternion(IntPtr body) + { + unsafe { return *(BodyGetQuaternionUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body); + public static Matrix3 BodyGetRotation(IntPtr body) + { + unsafe { return *(BodyGetRotationUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body); + public static Vector3 BodyGetTorque(IntPtr body) + { + unsafe { return *(BodyGetTorqueUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetWorld(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetFirstGeom(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr dBodyGetNextGeom(IntPtr Geom); + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyIsEnabled(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableSteps(IntPtr body, int steps); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableTime(IntPtr body, dReal time); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetData(IntPtr body, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationMode(IntPtr body, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearDamping(IntPtr body, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularDamping(IntPtr body, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetLinearDamping(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAngularDamping(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetLinearDampingThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAngularDampingThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetGravityMode(IntPtr body, bool mode); + + /// + /// Sets the Gyroscopic term status on the body specified. + /// + /// Pointer to body + /// NonZero enabled, Zero disabled + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity] + public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetMass(IntPtr body, ref Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref dReal w); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 p2, + ref Matrix3 R2, ref Vector3 side2, + ref Vector3 normal, out dReal depth, out int return_code, + int maxc, out ContactGeom contact, int skip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 _p2, + ref Matrix3 R2, ref Vector3 side2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity] + public static extern void CleanupODEAllDataForThread(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity] + public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2, + ref Vector3 b1, ref Vector3 b2, + ref Vector3 cp1, ref Vector3 cp2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity] + public static extern void CloseODE(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateGeom(int classnum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity] + public static extern int CreateGeomClass(ref GeomClass classptr); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateGeomTransform(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateRay(IntPtr space, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateSphere(IntPtr space, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] + public static extern dReal Dot(ref dReal X0, ref dReal X1, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity] + public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity] + public static extern int FactorCholesky(ref dReal A00, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity] + public static extern void GeomClearOffset(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyPosition(IntPtr geom, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyRotation(IntPtr geom, out dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomDestroy(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomDisable(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomEnable(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out dReal minX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetBody(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern int GeomGetCategoryBits(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetClassData(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern int GeomGetCollideBits(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity] + public static extern GeomClassID GeomGetClass(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetData(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetOffsetPosition(IntPtr geom) + { + unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetOffsetRotation(IntPtr geom) + { + unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetPosition(IntPtr geom) + { + unsafe { return *(GeomGetPositionUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetRotation(IntPtr geom) + { + unsafe { return *(GeomGetRotationUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetSpace(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsEnabled(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsOffset(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsSpace(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern int GeomRayGetClosestHit(IntPtr ray); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetLength(IntPtr ray); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetLength(IntPtr ray, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetBody(IntPtr geom, IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCategoryBits(IntPtr geom, int bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCollideBits(IntPtr geom, int bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetData(IntPtr geom, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSphereGetRadius(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetCleanup(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTransformGetGeom(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetInfo(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetCleanup(IntPtr geom, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetInfo(IntPtr geom, int info); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble1(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + double[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + dReal[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + float[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshClearTCCache(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataPreprocess(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataUpdate(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriCallback GeomTriMeshGetCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom); + public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom) + { + unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity] + public extern static int GeomTriMeshGetTriangleCount(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity] + public static extern string GetConfiguration(string str); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr HashSpaceCreate(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity] + public static extern void InfiniteAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity] + public static extern void InitODE(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity] + public static extern int InitODE2(uint ODEInitFlags); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity] + public static extern int IsPositiveDefinite(ref dReal A, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity] + public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHingeTorque(IntPtr joint, dReal torque); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddPRTorque(IntPtr joint, dReal torque); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddSliderForce(IntPtr joint, dReal force); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity] + public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointDestroy(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngle(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorAxisRel(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorMode(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorNumAxes(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetBody(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetData(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity] + public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j); + public static JointFeedback JointGetFeedback(IntPtr j) + { + unsafe { return *(JointGetFeedbackUnsafe(j)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngle(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngleRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle2Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Param(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetLMotorNumAxes(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetLMotorParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPosition(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPositionRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPosition(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPositionRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity] + public static extern JointType JointGetType(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGroupCreate(int max_size); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupDestroy(IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupEmpty(IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorMode(IntPtr j, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorNumAxes(IntPtr group, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetData(IntPtr j, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFixed(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorNumAxes(IntPtr j, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity] + public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdd(ref Mass a, ref Mass b); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdjust(ref Mass m, dReal newmass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity] + public static extern bool MassCheck(ref Mass m); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(ref Mass mass, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(ref Mass mass, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetParameters(out Mass mass, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal i11, dReal i22, dReal i33, + dReal i12, dReal i13, dReal i23); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity] + public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetZero(out Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity] + public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r); + public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector) + { + MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity] + public static extern void QfromR(out Quaternion q, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void QSetIdentity(out Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity] + public static extern dReal RandReal(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity] + public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity] + public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] + public static extern void RfromQ(out Matrix3 R, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity] + public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void RSetIdentity(out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity] + public static extern void SetValue(out dReal a, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void SetZero(out dReal a, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SimpleSpaceCreate(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity] + public static extern void SolveCholesky(ref dReal L, out dReal b, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceAdd(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceLockQuery(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceClean(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceDestroy(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceGetCleanup(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetNumGeoms(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SpaceGetGeom(IntPtr space, int i); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetSublevel(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceQuery(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceRemove(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetCleanup(IntPtr space, bool mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetSublevel(IntPtr space, int sublevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity] + public static extern void VectorScale(out dReal a, ref dReal d, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr WorldCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void WorldDestroy(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool WorldGetAutoDisableFlag(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableSteps(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableTime(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoEnableDepthSF1(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetCFM(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetERP(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactSurfaceLayer(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAngularDamping(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAngularDampingThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetLinearDamping(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetLinearDampingThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetQuickStepNumIterations(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetQuickStepW(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetMaxAngularSpeed(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldQuickStep(IntPtr world, dReal stepsize); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAngularDamping(IntPtr world, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetCFM(IntPtr world, dReal cfm); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetERP(IntPtr world, dReal erp); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetLinearDamping(IntPtr world, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStep(IntPtr world, dReal stepsize); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity] + public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix); + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs new file mode 100644 index 0000000..215d47a --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs @@ -0,0 +1,86 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// ODE plugin + /// + public class OdePlugin : IPhysicsPlugin + { + //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); + + private OdeScene m_scene; + + public bool Init() + { + if (d.InitODE2(0) != 0) + { + if (d.AllocateODEDataForThread(~0U) == 0) + { + d.CloseODE(); + return false; + } + return true; + } + return false; + } + + public PhysicsScene GetScene(String sceneIdentifier) + { + if (m_scene == null) + { + m_scene = new OdeScene(sceneIdentifier); + } + return (m_scene); + } + + public string GetName() + { + return ("UbitODE"); + } + + public void Dispose() + { + d.CloseODE(); + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs new file mode 100644 index 0000000..74de2ee --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -0,0 +1,2741 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +//#define USE_DRAWSTUFF +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using OdeAPI; +#if USE_DRAWSTUFF +using ODEDrawstuff; +#endif +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public enum StatusIndicators : int + { + Generic = 0, + Start = 1, + End = 2 + } + + public struct sCollisionData + { + public uint ColliderLocalId; + public uint CollidedWithLocalId; + public int NumberOfCollisions; + public int CollisionType; + public int StatusIndicator; + public int lastframe; + } + + [Flags] + public enum CollisionCategories : int + { + Disabled = 0, + Geom = 0x00000001, + Body = 0x00000002, + Space = 0x00000004, + Character = 0x00000008, + Land = 0x00000010, + Water = 0x00000020, + Wind = 0x00000040, + Sensor = 0x00000080, + Selected = 0x00000100 + } + + /// + /// Material type for a primitive + /// + public enum Material : int + { + /// + Stone = 0, + /// + Metal = 1, + /// + Glass = 2, + /// + Wood = 3, + /// + Flesh = 4, + /// + Plastic = 5, + /// + Rubber = 6, + + light = 7 // compatibility with old viewers + } + + public enum changes : int + { + Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) + Remove, + Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root + // or removes from a object if arg is null + DeLink, + Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child + Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child + PosOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + OriOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + Velocity, + AngVelocity, + Acceleration, + Force, + Torque, + + AddForce, + AddAngForce, + AngLock, + + Size, + Shape, + + CollidesWater, + VolumeDtc, + + Physical, + Selected, + disabled, + building, + + Null //keep this last used do dim the methods array. does nothing but pulsing the prim + } + + public struct ODEchangeitem + { + public OdePrim prim; + public OdeCharacter character; + public changes what; + public Object arg; + } + + public class OdeScene : PhysicsScene + { + private readonly ILog m_log; + // private Dictionary m_storedCollisions = new Dictionary(); + + private int threadid = 0; + private Random fluidRandomizer = new Random(Environment.TickCount); + + const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; + const float comumContactERP = 0.6f; + const float comumSoftContactERP = 0.1f; + const float comumContactCFM = 0.0001f; + + float frictionScale = 1.0f; + + float frictionMovementMult = 0.3f; + + float TerrainBounce = 0.3f; + float TerrainFriction = 0.3f; + + public float AvatarBounce = 0.3f; + public float AvatarFriction = 0;// 0.9f * 0.5f; + + private const uint m_regionWidth = Constants.RegionSize; + private const uint m_regionHeight = Constants.RegionSize; + + public float ODE_STEPSIZE = 0.020f; + private float metersInSpace = 25.6f; + private float m_timeDilation = 1.0f; + + public float gravityx = 0f; + public float gravityy = 0f; + public float gravityz = -9.8f; + + + private float waterlevel = 0f; + private int framecount = 0; + + internal IntPtr WaterGeom; + + public float avPIDD = 3200f; // make it visible + public float avPIDP = 1400f; // make it visible + private float avCapRadius = 0.37f; + private float avDensity = 3f; + private float avMovementDivisorWalk = 1.3f; + private float avMovementDivisorRun = 0.8f; + private float minimumGroundFlightOffset = 3f; + public float maximumMassObject = 10000.01f; + + public bool meshSculptedPrim = true; + public bool forceSimplePrimMeshing = false; + + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 16; + + public float geomDefaultDensity = 10.000006836f; + + public int geomContactPointsStartthrottle = 3; + public int geomUpdatesPerThrottledUpdate = 15; + + public float bodyPIDD = 35f; + public float bodyPIDG = 25; + + public int geomCrossingFailuresBeforeOutofbounds = 6; + + public int bodyFramesAutoDisable = 20; + + private float[] _watermap; + private bool m_filterCollisions = true; + + private d.NearCallback nearCallback; + + private readonly HashSet _characters = new HashSet(); + private readonly HashSet _prims = new HashSet(); + private readonly HashSet _activeprims = new HashSet(); + + private readonly Object _taintedCharacterLock = new Object(); + private readonly HashSet _taintedCharacterH = new HashSet(); // faster verification of repeated character taints + private readonly Queue _taintedCharacterQ = new Queue(); // character taints + + public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); + + /// + /// A list of actors that should receive collision events. + /// + private readonly List _collisionEventPrim = new List(); + + private readonly HashSet _badCharacter = new HashSet(); + public Dictionary geom_name_map = new Dictionary(); + public Dictionary actor_name_map = new Dictionary(); + + private float contactsurfacelayer = 0.002f; + + private int contactsPerCollision = 80; + internal IntPtr ContactgeomsArray = IntPtr.Zero; + private IntPtr GlobalContactsArray = IntPtr.Zero; + + const int maxContactsbeforedeath = 4000; + private volatile int m_global_contactcount = 0; + + + private readonly IntPtr contactgroup; + + public ContactData[] m_materialContactsData = new ContactData[8]; + + private readonly DoubleDictionary RegionTerrain = new DoubleDictionary(); + private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); + private readonly Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); + + private int m_physicsiterations = 10; + private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag + private readonly PhysicsActor PANull = new NullPhysicsActor(); + private float step_time = 0.0f; + + public IntPtr world; + + private uint obj2LocalID = 0; + private OdeCharacter cc1; + private OdePrim cp1; + private OdeCharacter cc2; + private OdePrim cp2; + + // split the spaces acording to contents type + // ActiveSpace contains characters and active prims + // StaticSpace contains land and other that is mostly static in enviroment + // this can contain subspaces, like the grid in staticspace + // as now space only contains this 2 top spaces + + public IntPtr TopSpace; // the global space + public IntPtr ActiveSpace; // space for active prims + public IntPtr StaticSpace; // space for the static things around + + // some speedup variables + private int spaceGridMaxX; + private int spaceGridMaxY; + private float spacesPerMeter; + + // split static geometry collision into a grid as before + private IntPtr[,] staticPrimspace; + + private Object OdeLock; + private static Object SimulationLock; + + public IMesher mesher; + + private IConfigSource m_config; + + public bool physics_logging = false; + public int physics_logging_interval = 0; + public bool physics_logging_append_existing_logfile = false; + + private Vector3 m_worldOffset = Vector3.Zero; + public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + private PhysicsScene m_parentScene = null; + + private ODERayCastRequestManager m_rayCastManager; + + +/* maybe needed if ode uses tls + private void checkThread() + { + + int th = Thread.CurrentThread.ManagedThreadId; + if(th != threadid) + { + threadid = th; + d.AllocateODEDataForThread(~0U); + } + } + */ + /// + /// Initiailizes the scene + /// Sets many properties that ODE requires to be stable + /// These settings need to be tweaked 'exactly' right or weird stuff happens. + /// + public OdeScene(string sceneIdentifier) + { + m_log + = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); + +// checkThread(); + Name = sceneIdentifier; + + OdeLock = new Object(); + SimulationLock = new Object(); + + nearCallback = near; + + m_rayCastManager = new ODERayCastRequestManager(this); + lock (OdeLock) + { + // Create the world and the first space + try + { + world = d.WorldCreate(); + TopSpace = d.HashSpaceCreate(IntPtr.Zero); + + // now the major subspaces + ActiveSpace = d.HashSpaceCreate(TopSpace); + StaticSpace = d.HashSpaceCreate(TopSpace); + } + catch + { + // i must RtC#FM + } + + d.HashSpaceSetLevels(TopSpace, -2, 8); // cell sizes from .25 to 256 ?? need check what this really does + d.HashSpaceSetLevels(ActiveSpace, -2, 8); + d.HashSpaceSetLevels(StaticSpace, -2, 8); + + // demote to second level + d.SpaceSetSublevel(ActiveSpace, 1); + d.SpaceSetSublevel(StaticSpace, 1); + + contactgroup = d.JointGroupCreate(0); + //contactgroup + + d.WorldSetAutoDisableFlag(world, false); + #if USE_DRAWSTUFF + + Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); + viewthread.Start(); + #endif + } + + _watermap = new float[258 * 258]; + } + +#if USE_DRAWSTUFF + public void startvisualization(object o) + { + ds.Functions fn; + fn.version = ds.VERSION; + fn.start = new ds.CallbackFunction(start); + fn.step = new ds.CallbackFunction(step); + fn.command = new ds.CallbackFunction(command); + fn.stop = null; + fn.path_to_textures = "./textures"; + string[] args = new string[0]; + ds.SimulationLoop(args.Length, args, 352, 288, ref fn); + } +#endif + + // Initialize the mesh plugin +// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region ) + public override void Initialise(IMesher meshmerizer, IConfigSource config) + { +// checkThread(); + mesher = meshmerizer; + m_config = config; + +// m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE")); + /* + if (region != null) + { + WorldExtents.X = region.RegionSizeX; + WorldExtents.Y = region.RegionSizeY; + } + */ + + // Defaults + + avPIDD = 2200.0f; + avPIDP = 900.0f; + + int contactsPerCollision = 80; + + if (m_config != null) + { + IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + if (physicsconfig != null) + { + gravityx = physicsconfig.GetFloat("world_gravityx", 0f); + gravityy = physicsconfig.GetFloat("world_gravityy", 0f); + gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f); + + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); + + contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); + + ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f); + m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); + + avDensity = physicsconfig.GetFloat("av_density", avDensity); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); + avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f); + + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); + + geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); + geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); + geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); + + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); + bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); + + bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f); + bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f); + + forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); + meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); + meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f); + MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f); + m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f); + } + else + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); + } + + physics_logging = physicsconfig.GetBoolean("physics_logging", false); + physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); + physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); + + minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); + } + } + + ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); + GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); + + m_materialContactsData[(int)Material.Stone].mu = frictionScale * 0.8f; + m_materialContactsData[(int)Material.Stone].bounce = 0.4f; + + m_materialContactsData[(int)Material.Metal].mu = frictionScale * 0.3f; + m_materialContactsData[(int)Material.Metal].bounce = 0.4f; + + m_materialContactsData[(int)Material.Glass].mu = frictionScale * 0.2f; + m_materialContactsData[(int)Material.Glass].bounce = 0.7f; + + m_materialContactsData[(int)Material.Wood].mu = frictionScale * 0.6f; + m_materialContactsData[(int)Material.Wood].bounce = 0.5f; + + m_materialContactsData[(int)Material.Flesh].mu = frictionScale * 0.9f; + m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; + + m_materialContactsData[(int)Material.Plastic].mu = frictionScale * 0.4f; + m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; + + m_materialContactsData[(int)Material.Rubber].mu = frictionScale * 0.9f; + m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; + + m_materialContactsData[(int)Material.light].mu = 0.0f; + m_materialContactsData[(int)Material.light].bounce = 0.0f; + + TerrainFriction *= frictionScale; +// AvatarFriction *= frictionScale; + + // Set the gravity,, don't disable things automatically (we set it explicitly on some things) + + d.WorldSetGravity(world, gravityx, gravityy, gravityz); + d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); + + d.WorldSetLinearDamping(world, 0.001f); + d.WorldSetAngularDamping(world, 0.001f); + d.WorldSetAngularDampingThreshold(world, 0f); + d.WorldSetLinearDampingThreshold(world, 0f); + d.WorldSetMaxAngularSpeed(world, 256f); + + d.WorldSetCFM(world,1e-6f); // a bit harder than default + //d.WorldSetCFM(world, 1e-4f); // a bit harder than default + d.WorldSetERP(world, 0.6f); // higher than original + + // Set how many steps we go without running collision testing + // This is in addition to the step size. + // Essentially Steps * m_physicsiterations + d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + d.WorldSetContactMaxCorrectingVel(world, 100.0f); + + spacesPerMeter = 1 / metersInSpace; + spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); + spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter); + + staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; + + // create all spaces now + int i, j; + IntPtr newspace; + for (i = 0; i < spaceGridMaxX; i++) + for (j = 0; j < spaceGridMaxY; j++) + { + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + staticPrimspace[i, j] = newspace; + } + // let this now be real maximum values + spaceGridMaxX--; + spaceGridMaxY--; + } + + internal void waitForSpaceUnlock(IntPtr space) + { + //if (space != IntPtr.Zero) + //while (d.SpaceLockQuery(space)) { } // Wait and do nothing + } + + #region Collision Detection + + // sets a global contact for a joint for contactgeom , and base contact description) + + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, bool softerp) + { + if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) + return IntPtr.Zero; + + d.Contact newcontact = new d.Contact(); + newcontact.geom.depth = contactGeom.depth; + newcontact.geom.g1 = contactGeom.g1; + newcontact.geom.g2 = contactGeom.g2; + newcontact.geom.pos = contactGeom.pos; + newcontact.geom.normal = contactGeom.normal; + newcontact.geom.side1 = contactGeom.side1; + newcontact.geom.side2 = contactGeom.side2; + + // this needs bounce also + newcontact.surface.mode = comumContactFlags; + newcontact.surface.mu = mu; + newcontact.surface.bounce = bounce; + newcontact.surface.soft_cfm = comumContactCFM; + if (softerp) + newcontact.surface.soft_erp = comumSoftContactERP; + else + newcontact.surface.soft_erp = comumContactERP; + + IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); + Marshal.StructureToPtr(newcontact, contact, true); + return d.JointCreateContactPtr(world, contactgroup, contact); + } + + + /// + /// This is our near callback. A geometry is near a body + /// + /// The space that contains the geoms. Remember, spaces are also geoms + /// a geometry or space + /// another geometry or space + /// + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) + return false; + + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; + } + + + + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + // no lock here! It's invoked from within Simulate(), which is thread-locked + + if (m_global_contactcount >= maxContactsbeforedeath) + return; + + // Test if we're colliding a geom with a space. + // If so we have to drill down into the space recursively + + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) + { + // We'll be calling near recursivly if one + // of them is a space to find all of the + // contact points in the space + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide test a space"); + return; + } + //here one should check collisions of geoms inside a space + // but on each space we only should have geoms that not colide amoung each other + // so we don't dig inside spaces + return; + } + + // get geom bodies to check if we already a joint contact + // guess this shouldn't happen now + IntPtr b1 = d.GeomGetBody(g1); + IntPtr b2 = d.GeomGetBody(g2); + + // d.GeomClassID id = d.GeomGetClass(g1); + + // Figure out how many contact points we have + int count = 0; + try + { + // Colliding Geom To Geom + // This portion of the function 'was' blatantly ripped off from BoxStack.cs + + if (g1 == g2) + return; // Can't collide with yourself + + if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) + return; + + count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + 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; + } + + // id contacts done + if (count == 0) + return; + + // try get physical actors + PhysicsActor p1; + PhysicsActor p2; + + if (!actor_name_map.TryGetValue(g1, out p1)) + { + p1 = PANull; + } + + if (!actor_name_map.TryGetValue(g2, out p2)) + { + p2 = PANull; + } + + // update actors collision score + if (p1.CollisionScore >= float.MaxValue - count) + p1.CollisionScore = 0; + p1.CollisionScore += count; + + if (p2.CollisionScore >= float.MaxValue - count) + p2.CollisionScore = 0; + p2.CollisionScore += count; + + + // get first contact + d.ContactGeom curContact = new d.ContactGeom(); + if (!GetCurContactGeom(0, ref curContact)) + return; + // for now it's the one with max depth + ContactPoint maxDepthContact = new ContactPoint( + new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), + new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), + curContact.depth + ); + // do volume detection case + if ( + (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) || + (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) + { + collision_accounting_events(p1, p2, maxDepthContact); + return; + } + + // big messy collision analises + float mu = 0; + float bounce = 0; + ContactData contactdata1; + ContactData contactdata2; + bool erpSoft = false; + + String name = null; + bool dop1foot = false; + bool dop2foot = false; + bool ignore = false; + + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Agent: + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + p1.CollidingObj = true; + p2.CollidingObj = true; + break; + case (int)ActorTypes.Prim: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + dop1foot = true; + break; + default: + ignore=true; // avatar to terrain and water ignored + break; + } + break; + + case (int)ActorTypes.Prim: + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + dop2foot = true; + if (p1.Velocity.LengthSquared() > 0.0f) + p1.CollidingObj = true; + break; + case (int)ActorTypes.Prim: + if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + erpSoft = true; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + break; + default: + if (geom_name_map.TryGetValue(g2, out name)) + { + if (name == "Terrain") + { + erpSoft = true; + contactdata1 = p1.ContactData; + bounce = contactdata1.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + p1.CollidingGround = true; + } + else if (name == "Water") + { + erpSoft = true; + } + } + else + ignore=true; + break; + } + break; + + default: + if (geom_name_map.TryGetValue(g1, out name)) + { + if (name == "Terrain") + { + if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + erpSoft = true; + p2.CollidingGround = true; + contactdata2 = p2.ContactData; + bounce = contactdata2.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + + if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + } + else + ignore = true; + + } + else if (name == "Water" && + (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent)) + { + erpSoft = true; + } + } + else + ignore = true; + break; + } + + if (ignore) + return; + + IntPtr Joint; + + int i = 0; + while(true) + { + if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) + p1.IsColliding = true; + if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) + p2.IsColliding = true; + + Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft); + d.JointAttach(Joint, b1, b2); + + if (++m_global_contactcount >= maxContactsbeforedeath) + break; + + if(++i >= count) + break; + + if (!GetCurContactGeom(i, ref curContact)) + break; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.SurfaceNormal.X = curContact.normal.X; + maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; + maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; + maxDepthContact.PenetrationDepth = curContact.depth; + } + } + + collision_accounting_events(p1, p2, maxDepthContact); + +/* + if (notskipedcount > geomContactPointsStartthrottle) + { + // If there are more then 3 contact points, it's likely + // that we've got a pile of objects, so ... + // We don't want to send out hundreds of terse updates over and over again + // so lets throttle them and send them again after it's somewhat sorted out. + this needs checking so out for now + if (b1 != IntPtr.Zero) + p1.ThrottleUpdates = true; + if (b2 != IntPtr.Zero) + p2.ThrottleUpdates = true; + + } + */ + } + + private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) + { + // obj1LocalID = 0; + //returncollisions = false; + obj2LocalID = 0; + //ctype = 0; + //cStartStop = 0; + if (!(p2.SubscribedEvents() || p1.SubscribedEvents())) + return; + + switch ((ActorTypes)p1.PhysicsActorType) + { + case ActorTypes.Agent: + cc1 = (OdeCharacter)p1; + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + cc2 = (OdeCharacter)p2; + obj2LocalID = cc2.m_localID; + if (p2.SubscribedEvents()) + cc2.AddCollisionEvent(cc1.m_localID, contact); + break; + + case ActorTypes.Prim: + if (p2 is OdePrim) + { + cp2 = (OdePrim)p2; + obj2LocalID = cp2.m_localID; + if (p2.SubscribedEvents()) + cp2.AddCollisionEvent(cc1.m_localID, contact); + } + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + obj2LocalID = 0; + break; + } + if (p1.SubscribedEvents()) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + cc1.AddCollisionEvent(obj2LocalID, contact); + } + break; + + case ActorTypes.Prim: + + if (p1 is OdePrim) + { + cp1 = (OdePrim)p1; + + // obj1LocalID = cp2.m_localID; + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + if (p2 is OdeCharacter) + { + cc2 = (OdeCharacter)p2; + obj2LocalID = cc2.m_localID; + if (p2.SubscribedEvents()) + cc2.AddCollisionEvent(cp1.m_localID, contact); + } + break; + case ActorTypes.Prim: + + if (p2 is OdePrim) + { + cp2 = (OdePrim)p2; + obj2LocalID = cp2.m_localID; + if (p2.SubscribedEvents()) + cp2.AddCollisionEvent(cp1.m_localID, contact); + } + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + obj2LocalID = 0; + break; + } + if (p1.SubscribedEvents()) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + cp1.AddCollisionEvent(obj2LocalID, contact); + } + } + break; + } + } + + /// + /// This is our collision testing routine in ODE + /// + /// + private void collision_optimized() + { +// _perloopContact.Clear(); +// clear characts IsColliding until we do it some other way + + lock (_characters) + { + foreach (OdeCharacter chr in _characters) + { + // this are odd checks if they are needed something is wrong elsewhere + // keep for now + if (chr == null) + continue; + + if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) + continue; + + chr.IsColliding = false; + // chr.CollidingGround = false; not done here + chr.CollidingObj = false; + } + } + + // now let ode do its job + // colide active things amoung them + + int st = Util.EnvironmentTickCount(); + int ta; + int ts; + try + { + d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to Active space collide"); + } + ta = Util.EnvironmentTickCountSubtract(st); + // then active things with static enviroment + try + { + d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to Active to static space collide"); + } + ts = Util.EnvironmentTickCountSubtract(st); +// _perloopContact.Clear(); + } + + #endregion + + + public float GetTerrainHeightAtXY(float x, float y) + { + // assumes 1m size grid and constante size square regions + // region offset in mega position + + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + + IntPtr heightFieldGeom = IntPtr.Zero; + + // get region map + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return 0f; + + if (heightFieldGeom == IntPtr.Zero) + return 0f; + + if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + return 0f; + + // TerrainHeightField for ODE as offset 1m + x += 1f - offsetX; + y += 1f - offsetY; + + // make position fit into array + if (x < 0) + x = 0; + if (y < 0) + y = 0; + + // integer indexs + int ix; + int iy; + // interpolators offset + float dx; + float dy; + + int regsize = (int)Constants.RegionSize + 2; // map size see setterrain + + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsize - 1; + dy = 0; + } + if (y < regsize - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsize - 1; + dx = 0; + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + + if ((dx + dy) <= 1.0f) + { + h0 = ((float)heights[iy]); // 0,0 vertice + h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + } + else + { + h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice + h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 + h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + } + + return h0 + h1 + h2; + } + + /// + /// Add actor to the list that should receive collision events in the simulate loop. + /// + /// + public void AddCollisionEventReporting(PhysicsActor obj) + { + lock (_collisionEventPrim) + { + if (!_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Add(obj); + } + } + + /// + /// Remove actor from the list that should receive collision events in the simulate loop. + /// + /// + public void RemoveCollisionEventReporting(PhysicsActor obj) + { + lock (_collisionEventPrim) + { + if (_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Remove(obj); + } + } + + #region Add/Remove Entities + + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) + { + Vector3 pos; + pos.X = position.X; + pos.Y = position.Y; + pos.Z = position.Z; + OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + newAv.Flying = isFlying; + newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; + + return newAv; + } + + public void AddCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (!_characters.Contains(chr)) + { + _characters.Add(chr); + if (chr.bad) + m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); + } + } + } + + public void RemoveCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (_characters.Contains(chr)) + { + _characters.Remove(chr); + } + } + } + + public void BadCharacter(OdeCharacter chr) + { + lock (_badCharacter) + { + if (!_badCharacter.Contains(chr)) + _badCharacter.Add(chr); + } + } + + public override void RemoveAvatar(PhysicsActor actor) + { + //m_log.Debug("[PHYSICS]:ODELOCK"); + ((OdeCharacter) actor).Destroy(); + } + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + PrimitiveBaseShape pbs, bool isphysical, uint localID) + { + Vector3 pos = position; + Vector3 siz = size; + Quaternion rot = rotation; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); + + lock (_prims) + _prims.Add(newPrim); + } + newPrim.LocalID = localID; + return newPrim; + } + + public void addActivePrim(OdePrim activatePrim) + { + // adds active prim.. (ones that should be iterated over in collisions_optimized + lock (_activeprims) + { + if (!_activeprims.Contains(activatePrim)) + _activeprims.Add(activatePrim); + //else + // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); + } + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + { +#if SPAM + m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); +#endif + + return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid); + } + + public override float TimeDilation + { + get { return m_timeDilation; } + } + + public override bool SupportsNINJAJoints + { + get { return false; } + } + + + public void remActivePrim(OdePrim deactivatePrim) + { + lock (_activeprims) + { + _activeprims.Remove(deactivatePrim); + } + } + + public override void RemovePrim(PhysicsActor prim) + { + // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be + // removed in the next physics simulate pass. + if (prim is OdePrim) + { +// lock (OdeLock) + { + OdePrim p = (OdePrim)prim; + p.setPrimForRemoval(); + } + } + } + /// + /// This is called from within simulate but outside the locked portion + /// We need to do our own locking here + /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in + /// Simulate() -- justincc). + /// + /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. + /// + /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory + /// that the space was using. + /// + /// + public void RemovePrimThreadLocked(OdePrim prim) + { + //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); + lock (prim) + { + RemoveCollisionEventReporting(prim); + lock (_prims) + _prims.Remove(prim); + } + + } + #endregion + + #region Space Separation Calculation + + /// + /// Called when a static prim moves or becomes static + /// Places the prim in a space one the static sub-spaces grid + /// + /// the pointer to the geom that moved + /// the position that the geom moved to + /// a pointer to the space it was in before it was moved. + /// a pointer to the new space it's in + public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) + { + // moves a prim into another static sub-space or from another space into a static sub-space + + // Called ODEPrim so + // it's already in locked space. + + if (geom == IntPtr.Zero) // shouldn't happen + return IntPtr.Zero; + + // get the static sub-space for current position + IntPtr newspace = calculateSpaceForGeom(pos); + + if (newspace == currentspace) // if we are there all done + return newspace; + + // else remove it from its current space + if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + else + { + m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + + " Geom:" + geom); + } + } + else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space + { + currentspace = d.GeomGetSpace(geom); + if (currentspace != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + } + } + + // put the geom in the newspace + waitForSpaceUnlock(newspace); + d.SpaceAdd(newspace, geom); + + // let caller know this newspace + return newspace; + } + + /// + /// Calculates the space the prim should be in by its position + /// + /// + /// a pointer to the space. This could be a new space or reused space. + public IntPtr calculateSpaceForGeom(Vector3 pos) + { + int x, y; + x = (int)(pos.X * spacesPerMeter); + if (x < 0) + x = 0; + else if (x > spaceGridMaxX) + x = spaceGridMaxX; + + y = (int)(pos.Y * spacesPerMeter); + if (y < 0) + y = 0; + else if (y >spaceGridMaxY) + y = spaceGridMaxY; + + IntPtr tmpSpace = staticPrimspace[x, y]; + return tmpSpace; + } + + #endregion + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// + public bool needsMeshing(PrimitiveBaseShape pbs) + { + // most of this is redundant now as the mesher will return null if it cant mesh a prim + // but we still need to check for sculptie meshing being enabled so this is the most + // convenient place to do it for now... + + // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f) + // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString()); + int iPropertiesNotSupportedDefault = 0; + + if (pbs.SculptEntry) + { + if(!meshSculptedPrim) + return false; + } + + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim + if (!forceSimplePrimMeshing && !pbs.SculptEntry) + { + if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) + || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 + && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) + { + + if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 + && pbs.ProfileHollow == 0 + && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 + && pbs.PathBegin == 0 && pbs.PathEnd == 0 + && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 + && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 + && pbs.PathShearX == 0 && pbs.PathShearY == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } + } + } + + // following code doesn't give meshs to boxes and spheres ever + // and it's odd.. so for now just return true if asked to force meshs + // hopefully mesher will fail if doesn't suport so things still get basic boxes + + if (forceSimplePrimMeshing) + return true; + + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + + if (pbs.SculptEntry && meshSculptedPrim) + iPropertiesNotSupportedDefault++; + + if (iPropertiesNotSupportedDefault == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } +#if SPAM + m_log.Debug("Mesh"); +#endif + return true; + } + + public void AddChange(OdePrim prim, changes what, Object arg) + { + ODEchangeitem item = new ODEchangeitem(); + item.prim = prim; + item.what = what; + item.arg = arg; + ChangesQueue.Enqueue(item); + } + + /// + /// Called to queue a change to a prim + /// to use in place of old taint mechanism so changes do have a time sequence + /// + public void AddChange(OdeCharacter character, changes what, Object arg) + { + ODEchangeitem item = new ODEchangeitem(); + item.character = character; + item.what = what; + item.arg = arg; + ChangesQueue.Enqueue(item); + } + + /// + /// Called after our prim properties are set Scale, position etc. + /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex + /// This assures us that we have no race conditions + /// + /// + public override void AddPhysicsActorTaint(PhysicsActor prim) + { + if (prim is OdePrim) + { +/* OdePrim taintedprim = ((OdePrim) prim); + lock (_taintedPrimLock) + { + if (!(_taintedPrimH.Contains(taintedprim))) + { + _taintedPrimH.Add(taintedprim); // HashSet for searching + _taintedPrimQ.Enqueue(taintedprim); // List for ordered readout + } + } + */ + return; + } + else if (prim is OdeCharacter) + { + OdeCharacter taintedchar = ((OdeCharacter)prim); + lock (_taintedCharacterLock) + { + if (!(_taintedCharacterH.Contains(taintedchar))) + { + _taintedCharacterH.Add(taintedchar); + _taintedCharacterQ.Enqueue(taintedchar); + if (taintedchar.bad) + m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid); + } + } + } + } + + /// + /// This is our main simulate loop + /// It's thread locked by a Mutex in the scene. + /// It holds Collisions, it instructs ODE to step through the physical reactions + /// It moves the objects around in memory + /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) + /// + /// + /// + public override float Simulate(float timeStep) + { + int statstart; + int statchanges = 0; + int statchmove = 0; + int statactmove = 0; + int statray = 0; + int statcol = 0; + int statstep = 0; + int statmovchar = 0; + int statmovprim; + int totjcontact = 0; + + // acumulate time so we can reduce error + step_time += timeStep; + + if (step_time < ODE_STEPSIZE) + return 0; + + if (framecount >= int.MaxValue) + framecount = 0; + + framecount++; + + int curphysiteractions = m_physicsiterations; + + if (step_time >= m_SkipFramesAtms) + { + // if in trouble reduce step resolution + curphysiteractions /= 2; + } + + int nodeframes = 0; + +// checkThread(); + + lock (SimulationLock) + { + // adjust number of iterations per step + try + { + d.WorldSetQuickStepNumIterations(world, curphysiteractions); + } + catch (StackOverflowException) + { + m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); +// ode.drelease(world); + base.TriggerPhysicsBasedRestart(); + } + + + while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever + { + try + { + statstart = Util.EnvironmentTickCount(); + + // clear pointer/counter to contacts to pass into joints + m_global_contactcount = 0; + + // do characters requested changes + + OdeCharacter character; + int numtaints; + lock (_taintedCharacterLock) + { + numtaints = _taintedCharacterQ.Count; + // if (numtaints > 50) + // numtaints = 50; + while (numtaints > 0) + { + character = _taintedCharacterQ.Dequeue(); + character.ProcessTaints(ODE_STEPSIZE); + _taintedCharacterH.Remove(character); + numtaints--; + } + } + // do other objects requested changes + + ODEchangeitem item; + + if(ChangesQueue.Count >0) + { + int ttmpstart = Util.EnvironmentTickCount(); + int ttmp; + int ttmp2; + + while(ChangesQueue.Dequeue(out item)) + { + if (item.prim != null) + { + try + { + if (item.prim.DoAChange(item.what, item.arg)) + RemovePrimThreadLocked(item.prim); + } + catch { }; + } + ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp > 20) + break; + } + + ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp2 > 50) + ttmp2 = 0; + + } + + statchanges += Util.EnvironmentTickCountSubtract(statstart); + + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(ODE_STEPSIZE, defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + } + } + statchmove += Util.EnvironmentTickCountSubtract(statstart); + + // Move other active objects + lock (_activeprims) + { + foreach (OdePrim aprim in _activeprims) + { + aprim.CollisionScore = 0; + aprim.IsColliding = false; + aprim.Move(); + } + } + + statactmove += Util.EnvironmentTickCountSubtract(statstart); + //if ((framecount % m_randomizeWater) == 0) + // randomizeWater(waterlevel); + + m_rayCastManager.ProcessQueuedRequests(); + + statray += Util.EnvironmentTickCountSubtract(statstart); + collision_optimized(); + statcol += Util.EnvironmentTickCountSubtract(statstart); + + lock (_collisionEventPrim) + { + foreach (PhysicsActor obj in _collisionEventPrim) + { + if (obj == null) + continue; + + switch ((ActorTypes)obj.PhysicsActorType) + { + case ActorTypes.Agent: + OdeCharacter cobj = (OdeCharacter)obj; + cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f)); + cobj.SendCollisions(); + break; + + case ActorTypes.Prim: + OdePrim pobj = (OdePrim)obj; + pobj.SendCollisions(); + break; + } + } + } + + d.WorldQuickStep(world, ODE_STEPSIZE); + statstep += Util.EnvironmentTickCountSubtract(statstart); + d.JointGroupEmpty(contactgroup); + totjcontact += m_global_contactcount; + //ode.dunlock(world); + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); +// ode.dunlock(world); + } + + step_time -= ODE_STEPSIZE; + nodeframes++; + } + + statstart = Util.EnvironmentTickCount(); + + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); + + actor.UpdatePositionAndVelocity(); + } + } + } + + lock (_badCharacter) + { + if (_badCharacter.Count > 0) + { + foreach (OdeCharacter chr in _badCharacter) + { + RemoveCharacter(chr); + } + + _badCharacter.Clear(); + } + } + statmovchar = Util.EnvironmentTickCountSubtract(statstart); + + lock (_activeprims) + { + { + foreach (OdePrim actor in _activeprims) + { + if (actor.IsPhysical) + { + actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE); + } + } + } + } + + statmovprim = Util.EnvironmentTickCountSubtract(statstart); + + int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); + int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); + int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); + int nbodies = d.NTotalBodies; + + // Finished with all sim stepping. If requested, dump world state to file for debugging. + // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? + // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? + if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0)) + { + string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename + string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file + + if (physics_logging_append_existing_logfile) + { + string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------"; + TextWriter fwriter = File.AppendText(fname); + fwriter.WriteLine(header); + fwriter.Close(); + } + + d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); + } + + // think time dilation is not a physics issue alone.. but ok let's fake something + if (step_time < ODE_STEPSIZE) // we did the required loops + m_timeDilation = 1.0f; + else + { // we didn't forget the lost ones and let user know something + m_timeDilation = 1 - step_time / timeStep; + if (m_timeDilation < 0) + m_timeDilation = 0; + step_time = 0; + } + } + +// return nodeframes * ODE_STEPSIZE; // return real simulated time + return 1000 * nodeframes; // return steps for now * 1000 to keep core happy + } + + /// + public override void GetResults() + { + } + + public override bool IsThreaded + { + // for now we won't be multithreaded + get { return (false); } + } + + #region ODE Specific Terrain Fixes + public float[] ResizeTerrain512NearestNeighbour(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < WorldExtents.Y; y++) + { + for (int x = 0; x < WorldExtents.X; x++) + { + resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x]; + } + } + + // Resize using Nearest Neighbour + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512, 512]; + for (int y = 0; y < WorldExtents.Y; y++) + { + for (int x = 0; x < WorldExtents.X; x++) + { + resultarr2[y * 2, x * 2] = resultarr[y, x]; + + if (y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x]; + } + if (x < WorldExtents.X) + { + resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x]; + } + if (x < WorldExtents.X && y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x]; + } + } + } + + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) + { + if (resultarr2[y, x] <= 0) + returnarr[i] = 0.0000001f; + else + returnarr[i] = resultarr2[y, x]; + + i++; + } + } + + return returnarr; + } + + public float[] ResizeTerrain512Interpolation(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[512,512]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < 256; y++) + { + for (int x = 0; x < 256; x++) + { + resultarr[y, x] = heightMap[y * 256 + x]; + } + } + + // Resize using interpolation + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512,512]; + for (int y = 0; y < (int)Constants.RegionSize; y++) + { + for (int x = 0; x < (int)Constants.RegionSize; x++) + { + resultarr2[y*2, x*2] = resultarr[y, x]; + + if (y < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2); + } + } + else + { + resultarr2[(y*2) + 1, x*2] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2); + } + } + else + { + resultarr2[y*2, (x*2) + 1] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize) + { + if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize)) + { + resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; + } + } + } + } + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) + { + if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) + { + m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0"); + resultarr2[y, x] = 0; + } + returnarr[i] = resultarr2[y, x]; + i++; + } + } + + return returnarr; + } + + #endregion + + public override void SetTerrain(float[] heightMap) + { + if (m_worldOffset != Vector3.Zero && m_parentScene != null) + { + if (m_parentScene is OdeScene) + { + ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset); + } + } + else + { + SetTerrain(heightMap, m_worldOffset); + } + } + + public override void CombineTerrain(float[] heightMap, Vector3 pOffset) + { + SetTerrain(heightMap, pOffset); + } + + public void SetTerrain(float[] heightMap, Vector3 pOffset) + { + + float[] _heightmap; + _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; + + uint heightmapWidth = Constants.RegionSize + 2; + uint heightmapHeight = Constants.RegionSize + 2; + + uint heightmapWidthSamples; + + uint heightmapHeightSamples; + + heightmapWidthSamples = (uint)Constants.RegionSize + 2; + heightmapHeightSamples = (uint)Constants.RegionSize + 2; + + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 10f; + const int wrap = 0; + + int regionsize = (int) Constants.RegionSize + 2; + + float hfmin = float.MaxValue; + float hfmax = float.MinValue; + float val; + int xx; + int yy; + + int maxXXYY = regionsize - 3; + // flipping map adding one margin all around so things don't fall in edges + + int xt = 0; + xx = 0; + + for (int x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && xx < maxXXYY) + xx++; + yy = 0; + for (int y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxXXYY) + yy += (int)Constants.RegionSize; + + val = heightMap[yy + xx]; + _heightmap[xt + y] = val; + + if (hfmin > val) + hfmin = val; + if (hfmax < val) + hfmax = val; + + } + + xt += regionsize; + } + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + TerrainHeightFieldHeights.Remove(GroundGeom); + } + d.SpaceRemove(StaticSpace, GroundGeom); + d.GeomDestroy(GroundGeom); + } + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); + + d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[GroundGeom] = "Terrain"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + + q1 = q1 * q2; + + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(GroundGeom, ref R); + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0); + IntPtr testGround = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out testGround)) + { + RegionTerrain.Remove(pOffset); + } + RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); +// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + + } + } + + public override void DeleteTerrain() + { + } + + public float GetWaterLevel() + { + return waterlevel; + } + + public override bool SupportsCombining() + { + return true; + } +/* + public override void UnCombine(PhysicsScene pScene) + { + IntPtr localGround = IntPtr.Zero; +// float[] localHeightfield; + bool proceed = false; + List geomDestroyList = new List(); + + lock (OdeLock) + { + if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) + { + foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) + { + if (geom == localGround) + { +// localHeightfield = TerrainHeightFieldHeights[geom]; + proceed = true; + } + else + { + geomDestroyList.Add(geom); + } + } + + if (proceed) + { + m_worldOffset = Vector3.Zero; + WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + m_parentScene = null; + + foreach (IntPtr g in geomDestroyList) + { + // removingHeightField needs to be done or the garbage collector will + // collect the terrain data before we tell ODE to destroy it causing + // memory corruption + if (TerrainHeightFieldHeights.ContainsKey(g)) + { +// float[] removingHeightField = TerrainHeightFieldHeights[g]; + TerrainHeightFieldHeights.Remove(g); + + if (RegionTerrain.ContainsKey(g)) + { + RegionTerrain.Remove(g); + } + + d.GeomDestroy(g); + //removingHeightField = new float[0]; + } + } + + } + else + { + m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); + } + } + } + } +*/ + public override void SetWaterLevel(float baseheight) + { + waterlevel = baseheight; + randomizeWater(waterlevel); + } + + public void randomizeWater(float baseheight) + { + const uint heightmapWidth = m_regionWidth + 2; + const uint heightmapHeight = m_regionHeight + 2; + const uint heightmapWidthSamples = m_regionWidth + 2; + const uint heightmapHeightSamples = m_regionHeight + 2; + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 2.9f; + const int wrap = 0; + + for (int i = 0; i < (258 * 258); i++) + { + _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); + // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); + } + + lock (OdeLock) + { + if (WaterGeom != IntPtr.Zero) + { + d.SpaceRemove(StaticSpace, WaterGeom); + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); + d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); + WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); + if (WaterGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); + d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[WaterGeom] = "Water"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + q1 = q1 * q2; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(WaterGeom, ref R); + d.GeomSetPosition(WaterGeom, 128, 128, 0); + + } + + } + + public override void Dispose() + { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + + lock (OdeLock) + { + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + RemovePrim(prm); + } + } + + if (ContactgeomsArray != IntPtr.Zero) + Marshal.FreeHGlobal(ContactgeomsArray); + if (GlobalContactsArray != IntPtr.Zero) + Marshal.FreeHGlobal(GlobalContactsArray); + + d.WorldDestroy(world); + //d.CloseODE(); + } + } + + public override Dictionary GetTopColliders() + { + Dictionary returncolliders = new Dictionary(); + int cnt = 0; + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + if (prm.CollisionScore > 0) + { + returncolliders.Add(prm.m_localID, prm.CollisionScore); + cnt++; + prm.CollisionScore = 0f; + if (cnt > 25) + { + break; + } + } + } + } + return returncolliders; + } + + public override bool SupportsRayCast() + { + return true; + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null) + { + m_rayCastManager.QueueRequest(position, direction, length, retMethod); + } + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + { + if (retMethod != null) + { + m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); + } + } + + // don't like this + public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) + { + ContactResult[] ourResults = null; + RayCallback retMethod = delegate(List results) + { + ourResults = new ContactResult[results.Count]; + results.CopyTo(ourResults, 0); + }; + int waitTime = 0; + m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); + while (ourResults == null && waitTime < 1000) + { + Thread.Sleep(1); + waitTime++; + } + if (ourResults == null) + return new List(); + return new List(ourResults); + } + + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null && actor !=null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return; + if (geom == IntPtr.Zero) + return; + m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod); + } + } + + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + { + if (retMethod != null && actor != null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return; + if (geom == IntPtr.Zero) + return; + + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + } + } + + // don't like this + public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + { + if (actor != null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return new List(); + if (geom == IntPtr.Zero) + return new List(); + + ContactResult[] ourResults = null; + RayCallback retMethod = delegate(List results) + { + ourResults = new ContactResult[results.Count]; + results.CopyTo(ourResults, 0); + }; + int waitTime = 0; + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + while (ourResults == null && waitTime < 1000) + { + Thread.Sleep(1); + waitTime++; + } + if (ourResults == null) + return new List(); + return new List(ourResults); + } + return new List(); + } + +#if USE_DRAWSTUFF + // Keyboard callback + public void command(int cmd) + { + IntPtr geom; + d.Mass mass; + d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f); + + + + Char ch = Char.ToLower((Char)cmd); + switch ((Char)ch) + { + case 'w': + try + { + Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); + + xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z; + ds.SetViewpoint(ref xyz, ref hpr); + } + catch (ArgumentException) + { hpr.X = 0; } + break; + + case 'a': + hpr.X++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + + case 's': + try + { + Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); + + xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z; + ds.SetViewpoint(ref xyz, ref hpr); + } + catch (ArgumentException) + { hpr.X = 0; } + break; + case 'd': + hpr.X--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'r': + xyz.Z++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'f': + xyz.Z--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'e': + xyz.Y++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'q': + xyz.Y--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + } + } + + public void step(int pause) + { + + ds.SetColor(1.0f, 1.0f, 0.0f); + ds.SetTexture(ds.Texture.Wood); + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + //IntPtr body = d.GeomGetBody(prm.prim_geom); + if (prm.prim_geom != IntPtr.Zero) + { + d.Vector3 pos; + d.GeomCopyPosition(prm.prim_geom, out pos); + //d.BodyCopyPosition(body, out pos); + + d.Matrix3 R; + d.GeomCopyRotation(prm.prim_geom, out R); + //d.BodyCopyRotation(body, out R); + + + d.Vector3 sides = new d.Vector3(); + sides.X = prm.Size.X; + sides.Y = prm.Size.Y; + sides.Z = prm.Size.Z; + + ds.DrawBox(ref pos, ref R, ref sides); + } + } + } + ds.SetColor(1.0f, 0.0f, 0.0f); + lock (_characters) + { + foreach (OdeCharacter chr in _characters) + { + if (chr.Shell != IntPtr.Zero) + { + IntPtr body = d.GeomGetBody(chr.Shell); + + d.Vector3 pos; + d.GeomCopyPosition(chr.Shell, out pos); + //d.BodyCopyPosition(body, out pos); + + d.Matrix3 R; + d.GeomCopyRotation(chr.Shell, out R); + //d.BodyCopyRotation(body, out R); + + ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f); + d.Vector3 sides = new d.Vector3(); + sides.X = 0.5f; + sides.Y = 0.5f; + sides.Z = 0.5f; + + ds.DrawBox(ref pos, ref R, ref sides); + } + } + } + } + + public void start(int unused) + { + ds.SetViewpoint(ref xyz, ref hpr); + } +#endif + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs b/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs new file mode 100644 index 0000000..aefad3a --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs @@ -0,0 +1,99 @@ +/* + * Copyright ODE + * Ode.NET - .NET bindings for ODE + * Jason Perkins (starkos@industriousone.com) + * Licensed under the New BSD + * Part of the OpenDynamicsEngine +Open Dynamics Engine +Copyright (c) 2001-2007, Russell L. Smith. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +Neither the names of ODE's copyright owner nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +using System; +using System.Runtime.InteropServices; +using OdeAPI; + +namespace ODEDrawstuff +{ +/*#if dDOUBLE + using dReal = System.Double; +#else + */ + using dReal = System.Single; +//#endif + + public static class ds + { + public const int VERSION = 2; + + public enum Texture + { + None, + Wood + } + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void CallbackFunction(int arg); + + [StructLayout(LayoutKind.Sequential)] + public struct Functions + { + public int version; + public CallbackFunction start; + public CallbackFunction step; + public CallbackFunction command; + public CallbackFunction stop; + public string path_to_textures; + } + + [DllImport("drawstuff", EntryPoint = "dsDrawBox")] + public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides); + + [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")] + public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius); + + [DllImport("drawstuff", EntryPoint = "dsDrawConvex")] + public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("drawstuff", EntryPoint = "dsSetColor")] + public static extern void SetColor(float red, float green, float blue); + + [DllImport("drawstuff", EntryPoint = "dsSetTexture")] + public static extern void SetTexture(Texture texture); + + [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")] + public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr); + + [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")] + public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn); + } +} -- cgit v1.1 From c75fa8b8a1c4ba9235074e84f9a363483c5220f7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 8 Feb 2012 15:28:13 +0000 Subject: changes in physics manager, needed for UbitODE --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 19 +++++++++++++++++++ OpenSim/Region/Physics/Manager/PhysicsScene.cs | 10 +++++++++- 2 files changed, 28 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 0587054..e1a68be 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -68,6 +68,17 @@ namespace OpenSim.Region.Physics.Manager } } + public struct ContactData + { + public float mu; + public float bounce; + + public ContactData(float _mu, float _bounce) + { + mu = _mu; + bounce = _bounce; + } + } /// /// Used to pass collision information to OnCollisionUpdate listeners. /// @@ -143,6 +154,14 @@ namespace OpenSim.Region.Physics.Manager get { return new NullPhysicsActor(); } } + + public virtual bool Building { get; set; } + + public virtual ContactData ContactData + { + get { return new ContactData(0, 0); } + } + public abstract bool Stopped { get; } public abstract Vector3 Size { get; set; } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 514d9ad..3db71e5 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -225,7 +225,7 @@ namespace OpenSim.Region.Physics.Manager } public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {} - + public virtual void CombineTerrain(float[] heightMap, Vector3 pOffset) {} public virtual void UnCombine(PhysicsScene pScene) {} /// @@ -263,5 +263,13 @@ namespace OpenSim.Region.Physics.Manager { return new List(); } + + public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){} + public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { } + public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + { + return new List(); + } + } } -- cgit v1.1 From 6af01f6767838235091b9e34cdaea05951d68f68 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 8 Feb 2012 23:14:53 +0000 Subject: initial introdution of physics actor building control. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 1a53c99..490c178 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2062,7 +2062,7 @@ namespace OpenSim.Region.Physics.OdePlugin SetInStaticSpace(this); } - m_building = false; // REMOVE THIS LATER +// m_building = false; // REMOVE THIS LATER if (m_isphysical && Body == IntPtr.Zero) -- cgit v1.1 From 7cf73cb92af7d00f9d7a98c1762162d597418a3b Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 10 Feb 2012 22:43:51 +0000 Subject: Changes to vehicles code etc. Includes some debug aids to remove later. --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 296 +++++---------------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 +- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 85 +++++- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 1 + 4 files changed, 145 insertions(+), 243 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 363cbef..6b323fb 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -629,36 +629,31 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in object frame - d.Vector3 dtorque = new d.Vector3();// actually angular aceleration until mult by Inertia in object frame - - bool doathing = false; + d.Vector3 dtorque = new d.Vector3(); // linear motor if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) { tmpV = m_linearMotorDirection - curLocalVel; // velocity error - if (tmpV.LengthSquared() > 1e-6f) - { - tmpV = tmpV * (m_lmEfect / m_linearMotorTimescale); // error to correct in this timestep - tmpV *= rotq; // to world + tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep + tmpV *= rotq; // to world - if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) - tmpV.Z = 0; + if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) + tmpV.Z = 0; - if (m_linearMotorOffset.X != 0 && m_linearMotorOffset.Y != 0 && m_linearMotorOffset.Z != 0) - { - // have offset, do it now - tmpV *= rootPrim.Mass; - d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); - } - else - { - force.X += tmpV.X; - force.Y += tmpV.Y; - force.Z += tmpV.Z; - } + if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0) + { + // have offset, do it now + tmpV *= rootPrim.Mass; + d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); } - m_lmEfect *= (1 - 1.0f / m_linearMotorDecayTimescale); + else + { + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale); } else m_lmEfect = 0; @@ -719,30 +714,35 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_linearDeflectionEfficiency > 0) { float len = curVel.Length(); - Vector3 atAxis = refAtAxis; - atAxis *= rotq; // at axis rotated to world - atAxis = Xrot(rotq); - tmpV = atAxis * len; - tmpV -= curVel; // velocity error - tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep - force.X += tmpV.X; - force.Y += tmpV.Y; - if((m_flags & VehicleFlag.NO_DEFLECTION_UP) ==0) - force.Z += tmpV.Z; + Vector3 atAxis; + atAxis = Xrot(rotq); // where are we pointing to + atAxis *= len; // make it same size as world velocity vector + tmpV = -atAxis; // oposite direction + atAxis -= curVel; // error to one direction + len = atAxis.LengthSquared(); + tmpV -= curVel; // error to oposite + float lens = tmpV.LengthSquared(); + if (len > 0.01 || lens > 0.01) // do nothing if close enougth + { + if (len < lens) + tmpV = atAxis; + + tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep + force.X += tmpV.X; + force.Y += tmpV.Y; + if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) + force.Z += tmpV.Z; + } } // angular motor if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) { tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error - if (tmpV.LengthSquared() > 1e-6f) - { - tmpV = tmpV * (m_amEfect / m_angularMotorTimescale); // error to correct in this timestep - tmpV *= m_referenceFrame; // to object - dtorque.X += tmpV.X; - dtorque.Y += tmpV.Y; - dtorque.Z += tmpV.Z; - } + tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep + torque.X += tmpV.X; + torque.Y += tmpV.Y; + torque.Z += tmpV.Z; m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale); } else @@ -751,85 +751,64 @@ namespace OpenSim.Region.Physics.OdePlugin // angular friction if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) { - tmpV.X = -curLocalAngVel.X / m_angularFrictionTimescale.X; - tmpV.Y = -curLocalAngVel.Y / m_angularFrictionTimescale.Y; - tmpV.Z = -curLocalAngVel.Z / m_angularFrictionTimescale.Z; - tmpV *= m_referenceFrame; // to object - dtorque.X += tmpV.X; - dtorque.Y += tmpV.Y; - dtorque.Z += tmpV.Z; + torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X; + torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y; + torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z; } // angular deflection if (m_angularDeflectionEfficiency > 0) { - doathing = false; - float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale / m_angularDeflectionTimescale /_pParentScene.ODE_STEPSIZE; - tmpV.X = 0; - if (Math.Abs(curLocalVel.Z) > 0.01) + Vector3 dirv; + + if (curLocalVel.X > 0.01f) + dirv = curLocalVel; + else if (curLocalVel.X < -0.01f) + // use oposite + dirv = -curLocalVel; + else { - tmpV.Y = -(float)Math.Atan2(curLocalVel.Z, curLocalVel.X) * ftmp; - doathing = true; + // make it fall into small positive x case + dirv.X = 0.01f; + dirv.Y = curLocalVel.Y; + dirv.Z = curLocalVel.Z; } - else - tmpV.Y = 0; - if (Math.Abs(curLocalVel.Y) > 0.01) + + float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; + + if (Math.Abs(dirv.Z) > 0.01) { - tmpV.Z = (float)Math.Atan2(curLocalVel.Y, curLocalVel.X) * ftmp; - doathing = true; + torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; } - else - tmpV.Z = 0; - if (doathing) + if (Math.Abs(dirv.Y) > 0.01) { - tmpV *= m_referenceFrame; // to object - dtorque.X += tmpV.X; - dtorque.Y += tmpV.Y; - dtorque.Z += tmpV.Z; + torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; } } // vertical atractor if (m_verticalAttractionTimescale < 300) { - doathing = false; float roll; float pitch; GetRollPitch(rotq, out roll, out pitch); - float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; if (Math.Abs(roll) > 0.01) // roll { - tmpV.X = -roll * ftmp; - tmpV.X -= curLocalAngVel.X * ftmp2; - doathing = true; - } - else - { - tmpV.X = 0; + torque.X -= roll * ftmp + curLocalAngVel.X * ftmp2; } if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch { - tmpV.Y = -pitch * ftmp; - tmpV.Y -= curLocalAngVel.Y * ftmp2; - doathing = true; - } - else - { - tmpV.Y = 0; + torque.Y -= pitch * ftmp + curLocalAngVel.Y * ftmp2; } - tmpV.Z = 0; - - if (m_bankingEfficiency == 0 || Math.Abs(roll) < 0.01) - tmpV.Z = 0; - else + if (m_bankingEfficiency != 0 && Math.Abs(roll) < 0.01) { float broll = -roll * m_bankingEfficiency; ; if (m_bankingMix != 0) @@ -839,146 +818,10 @@ namespace OpenSim.Region.Physics.OdePlugin broll *= ((1 - m_bankingMix) + vfact); } - tmpV.Z = (broll - curLocalAngVel.Z) / m_bankingTimescale; - doathing = true; - } - - if (doathing) - { - - tmpV *= m_referenceFrame; // to object - dtorque.X += tmpV.X; - dtorque.Y += tmpV.Y; - dtorque.Z += tmpV.Z; + torque.Z += (broll - curLocalAngVel.Z) / m_bankingTimescale; } } - /* - d.Vector3 pos = d.BodyGetPosition(Body); - // Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f); - Vector3 posChange = new Vector3(); - posChange.X = pos.X - m_lastPositionVector.X; - posChange.Y = pos.Y - m_lastPositionVector.Y; - posChange.Z = pos.Z - m_lastPositionVector.Z; - double Zchange = Math.Abs(posChange.Z); - if (m_BlockingEndPoint != Vector3.Zero) - { - if (pos.X >= (m_BlockingEndPoint.X - (float)1)) - { - pos.X -= posChange.X + 1; - d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); - } - if (pos.Y >= (m_BlockingEndPoint.Y - (float)1)) - { - pos.Y -= posChange.Y + 1; - d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); - } - if (pos.Z >= (m_BlockingEndPoint.Z - (float)1)) - { - pos.Z -= posChange.Z + 1; - d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); - } - if (pos.X <= 0) - { - pos.X += posChange.X + 1; - d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); - } - if (pos.Y <= 0) - { - pos.Y += posChange.Y + 1; - d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); - } - } - if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y)) - { - pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2; - d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); - } - - } - if ((m_flags & (VehicleFlag.NO_X)) != 0) - { - m_dir.X = 0; - } - if ((m_flags & (VehicleFlag.NO_Y)) != 0) - { - m_dir.Y = 0; - } - if ((m_flags & (VehicleFlag.NO_Z)) != 0) - { - m_dir.Z = 0; - } - - - */ - // angular part - /* - - // Get what the body is doing, this includes 'external' influences -/* - Vector3 angularVelocity = Vector3.Zero; - - // Vertical attractor section - Vector3 vertattr = Vector3.Zero; - - if (m_verticalAttractionTimescale < 300) - { - float VAservo = 0.2f / m_verticalAttractionTimescale; - // get present body rotation - // make a vector pointing up - Vector3 verterr = Vector3.Zero; - verterr.Z = 1.0f; - // rotate it to Body Angle - verterr = verterr * rotq; - // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. - // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go - // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. - if (verterr.Z < 0.0f) - { - verterr.X = 2.0f - verterr.X; - verterr.Y = 2.0f - verterr.Y; - } - // Error is 0 (no error) to +/- 2 (max error) - // scale it by VAservo - verterr = verterr * VAservo; -//if (frcount == 0) Console.WriteLine("VAerr=" + verterr); - - // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so - // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. - vertattr.X = verterr.Y; - vertattr.Y = - verterr.X; - vertattr.Z = 0f; - - // scaling appears better usingsquare-law - float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); - vertattr.X += bounce * angularVelocity.X; - vertattr.Y += bounce * angularVelocity.Y; - - } // else vertical attractor is off - - // m_lastVertAttractor = vertattr; - - // Bank section tba - // Deflection section tba - - // Sum velocities - m_lastAngularVelocity = angularVelocity + vertattr; // + bank + deflection - if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) - { - m_lastAngularVelocity.X = 0; - m_lastAngularVelocity.Y = 0; - } - - if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) - { - if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - } - else - { - m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. - } -*/ - d.Mass dmass; d.BodyGetMass(Body,out dmass); @@ -988,8 +831,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetForce(Body, force.X, force.Y, force.Z); } - if (dtorque.X != 0 || dtorque.Y != 0 || dtorque.Z != 0) + if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) { + torque *= m_referenceFrame; // to object frame + dtorque.X = torque.X; + dtorque.Y = torque.Y; + dtorque.Z = torque.Z; + d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 490c178..7718a29 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1198,7 +1198,7 @@ namespace OpenSim.Region.Physics.OdePlugin } Body = IntPtr.Zero; hasOOBoffsetFromMesh = false; -// CalcPrimBodyData(); + CalcPrimBodyData(); } private void ChildSetGeom(OdePrim odePrim) @@ -1223,7 +1223,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) { - /* if (m_targetSpace != _parent_scene.ActiveSpace) { m_targetSpace = _parent_scene.ActiveSpace; @@ -1238,7 +1237,6 @@ namespace OpenSim.Region.Physics.OdePlugin } d.SpaceAdd(m_targetSpace, prim_geom); } - */ d.GeomEnable(prim_geom); foreach (OdePrim prm in childrenPrim) d.GeomEnable(prm.prim_geom); @@ -1256,7 +1254,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) { - /* if (m_targetSpace == _parent_scene.ActiveSpace) { foreach (OdePrim prm in childrenPrim) @@ -1270,7 +1267,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceRemove(m_targetSpace, prim_geom); m_targetSpace = IntPtr.Zero; } - */ d.GeomDisable(prim_geom); foreach (OdePrim prm in childrenPrim) d.GeomDisable(prm.prim_geom); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index c0c7ff3..2b6bc59 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -59,6 +59,7 @@ namespace OdeAPI { public static dReal Infinity = dReal.MaxValue; public static int NTotalBodies = 0; + public static int NTotalGeoms = 0; #region Flags and Enumerations @@ -420,7 +421,7 @@ namespace OdeAPI [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] public static extern IntPtr BodyiCreate(IntPtr world); - public static IntPtr BodyCreate(IntPtr world) + public static IntPtr BodyCreate(IntPtr world) { NTotalBodies++; return BodyiCreate(world); @@ -689,22 +690,52 @@ namespace OdeAPI public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz); + public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz); + public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz) + { + NTotalGeoms++; + return CreateiBox(space, lx, ly, lz); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length); + public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length); + public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length) + { + NTotalGeoms++; + return CreateiCapsule(space, radius, length); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons) + { + NTotalGeoms++; + return CreateiConvex(space, planes, planeCount, points, pointCount, polygons); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length); + public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length); + public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length) + { + NTotalGeoms++; + return CreateiCylinder(space, radius, length); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable); + public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiHeightfield(space, data, bPlaceable); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateGeom(int classnum); + public static extern IntPtr CreateiGeom(int classnum); + public static IntPtr CreateGeom(int classnum) + { + NTotalGeoms++; + return CreateiGeom(classnum); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity] public static extern int CreateGeomClass(ref GeomClass classptr); @@ -713,19 +744,39 @@ namespace OdeAPI public static extern IntPtr CreateGeomTransform(IntPtr space); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d) + { + NTotalGeoms++; + return CreateiPlane(space, a, b, c, d); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateRay(IntPtr space, dReal length); + public static extern IntPtr CreateiRay(IntPtr space, dReal length); + public static IntPtr CreateRay(IntPtr space, dReal length) + { + NTotalGeoms++; + return CreateiRay(space, length); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateSphere(IntPtr space, dReal radius); + public static extern IntPtr CreateiSphere(IntPtr space, dReal radius); + public static IntPtr CreateSphere(IntPtr space, dReal radius) + { + NTotalGeoms++; + return CreateiSphere(space, radius); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data, + public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] + public static IntPtr CreateTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback) + { + NTotalGeoms++; + return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback); + } + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] public static extern dReal Dot(ref dReal X0, ref dReal X1, int n); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity] @@ -798,7 +849,13 @@ namespace OdeAPI public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void GeomDestroy(IntPtr geom); + public static extern void GeomiDestroy(IntPtr geom); + public static void GeomDestroy(IntPtr geom) + { + NTotalGeoms--; + GeomiDestroy(geom); + } + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] public static extern void GeomDisable(IntPtr geom); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 74de2ee..e60b006 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1860,6 +1860,7 @@ namespace OpenSim.Region.Physics.OdePlugin int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); int nbodies = d.NTotalBodies; + int ngeoms = d.NTotalGeoms; // Finished with all sim stepping. If requested, dump world state to file for debugging. // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? -- cgit v1.1 From 43a74d8481ff958d9642d2759915fdee6464ab1d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 10 Feb 2012 23:51:55 +0000 Subject: test on git --- OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 1 + 4 files changed, 4 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 6b323fb..0272f34 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -1,3 +1,4 @@ + /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 7718a29..b0dbe50 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1,3 +1,4 @@ + /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 2b6bc59..e62746e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -1,3 +1,4 @@ + /* * based on: * Ode.NET - .NET bindings for ODE diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index e60b006..fc59c32 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1,3 +1,4 @@ + /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. -- cgit v1.1 From b617411b972281529684bd8151a59899c81bfcf6 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 02:48:38 +0000 Subject: scale avatar push force with avatar density --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index c8f7c76..b0b91f6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -768,7 +768,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (pushforce) { m_pidControllerActive = false; - m_taintForce = force / _parent_scene.ODE_STEPSIZE; + // scale with odetime step and density + m_taintForce = force * m_density / _parent_scene.ODE_STEPSIZE / 28f; m_hasTaintForce = true; _parent_scene.AddPhysicsActorTaint(this); } -- cgit v1.1 From f415256e0b0d4b0191d52cb84090a0f1b0044ae9 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 03:25:17 +0000 Subject: Use mesh to estimate real center of prims if avaiable. Let sculpt map textures with alpha channel work. On linux J2DecodeCache folder must be deleted to remove bad entries. Corrently this can't be cached on linux (mono/ cairo/? problem) --- OpenSim/Region/Physics/Manager/IMesher.cs | 1 + OpenSim/Region/Physics/Meshing/Mesh.cs | 72 ++++++++++++++++++++++++- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 21 ++++++-- OpenSim/Region/Physics/Meshing/SculptMap.cs | 62 ++++++++++++--------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 +-- 5 files changed, 129 insertions(+), 33 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index 3a9ca1b..cc92484 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -65,5 +65,6 @@ namespace OpenSim.Region.Physics.Manager void releasePinned(); void Append(IMesh newMesh); void TransformLinear(float[,] matrix, float[] offset); + Vector3 GetCentroid(); } } diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs index f781ff9..c715642 100644 --- a/OpenSim/Region/Physics/Meshing/Mesh.cs +++ b/OpenSim/Region/Physics/Meshing/Mesh.cs @@ -46,11 +46,36 @@ namespace OpenSim.Region.Physics.Meshing IntPtr m_indicesPtr = IntPtr.Zero; int m_indexCount = 0; public float[] m_normals; + Vector3 _centroid; + int _centroidDiv; + + private class vertexcomp : IEqualityComparer + { + public bool Equals(Vertex v1, Vertex v2) + { + if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z) + return true; + else + return false; + } + public int GetHashCode(Vertex v) + { + int a = v.X.GetHashCode(); + int b = v.Y.GetHashCode(); + int c = v.Z.GetHashCode(); + return (a << 16) ^ (b << 8) ^ c; + } + + } public Mesh() { - m_vertices = new Dictionary(); + vertexcomp vcomp = new vertexcomp(); + + m_vertices = new Dictionary(vcomp); m_triangles = new List(); + _centroid = Vector3.Zero; + _centroidDiv = 0; } public Mesh Clone() @@ -61,7 +86,8 @@ namespace OpenSim.Region.Physics.Meshing { result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); } - + result._centroid = _centroid; + result._centroidDiv = _centroidDiv; return result; } @@ -71,15 +97,57 @@ namespace OpenSim.Region.Physics.Meshing throw new NotSupportedException("Attempt to Add to a pinned Mesh"); // If a vertex of the triangle is not yet in the vertices list, // add it and set its index to the current index count + // vertex == seems broken + // skip colapsed triangles + if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) + || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) + || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z) + ) + { + return; + } + + if (m_vertices.Count == 0) + { + _centroidDiv = 0; + _centroid = Vector3.Zero; + } + if (!m_vertices.ContainsKey(triangle.v1)) + { m_vertices[triangle.v1] = m_vertices.Count; + _centroid.X += triangle.v1.X; + _centroid.Y += triangle.v1.Y; + _centroid.Z += triangle.v1.Z; + _centroidDiv++; + } if (!m_vertices.ContainsKey(triangle.v2)) + { m_vertices[triangle.v2] = m_vertices.Count; + _centroid.X += triangle.v2.X; + _centroid.Y += triangle.v2.Y; + _centroid.Z += triangle.v2.Z; + _centroidDiv++; + } if (!m_vertices.ContainsKey(triangle.v3)) + { m_vertices[triangle.v3] = m_vertices.Count; + _centroid.X += triangle.v3.X; + _centroid.Y += triangle.v3.Y; + _centroid.Z += triangle.v3.Z; + _centroidDiv++; + } m_triangles.Add(triangle); } + public Vector3 GetCentroid() + { + if (_centroidDiv > 0) + return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv); + else + return Vector3.Zero; + } + public void CalcNormals() { int iTriangles = m_triangles.Count; diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index f15e81b..ba461f7 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -74,6 +74,8 @@ namespace OpenSim.Region.Physics.Meshing #endif private bool cacheSculptMaps = true; + private bool cacheSculptAlphaMaps = true; + private string decodedSculptMapPath = null; private bool useMeshiesPhysicsMesh = false; @@ -87,7 +89,16 @@ namespace OpenSim.Region.Physics.Meshing IConfig mesh_config = config.Configs["Mesh"]; decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); + cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + cacheSculptAlphaMaps = false; + } + else + cacheSculptAlphaMaps = cacheSculptMaps; + if(mesh_config != null) useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); @@ -268,15 +279,18 @@ namespace OpenSim.Region.Physics.Meshing { if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) return null; + // Remove the reference to any JPEG2000 sculpt data so it can be GCed + // don't loose it + // primShape.SculptData = Utils.EmptyBytes; } +// primShape.SculptDataLoaded = true; } else { if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) return null; } - - // Remove the reference to any JPEG2000 sculpt data so it can be GCed + // keep compatible primShape.SculptData = Utils.EmptyBytes; int numCoords = coords.Count; @@ -482,7 +496,8 @@ namespace OpenSim.Region.Physics.Meshing //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData); - if (cacheSculptMaps) + if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0))) + // don't cache images with alpha channel in linux since mono can't load them correctly) { try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); } catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); } diff --git a/OpenSim/Region/Physics/Meshing/SculptMap.cs b/OpenSim/Region/Physics/Meshing/SculptMap.cs index 740424e..b3d9cb6 100644 --- a/OpenSim/Region/Physics/Meshing/SculptMap.cs +++ b/OpenSim/Region/Physics/Meshing/SculptMap.cs @@ -58,28 +58,24 @@ namespace PrimMesher if (bmW == 0 || bmH == 0) throw new Exception("SculptMap: bitmap has no data"); - int numLodPixels = lod * 2 * lod * 2; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + bool smallMap = bmW * bmH <= numLodPixels; bool needsScaling = false; - bool smallMap = bmW * bmH <= lod * lod; - width = bmW; height = bmH; - while (width * height > numLodPixels) + while (width * height > numLodPixels * 4) { width >>= 1; height >>= 1; needsScaling = true; } - - try { if (needsScaling) - bm = ScaleImage(bm, width, height, - System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor); + bm = ScaleImage(bm, width, height); } catch (Exception e) @@ -87,7 +83,7 @@ namespace PrimMesher throw new Exception("Exception in ScaleImage(): e: " + e.ToString()); } - if (width * height > lod * lod) + if (width * height > numLodPixels) { width >>= 1; height >>= 1; @@ -144,15 +140,17 @@ namespace PrimMesher int rowNdx, colNdx; int smNdx = 0; + for (rowNdx = 0; rowNdx < numRows; rowNdx++) { List row = new List(numCols); for (colNdx = 0; colNdx < numCols; colNdx++) { + if (mirror) - row.Add(new Coord(-(redBytes[smNdx] * pixScale - 0.5f), (greenBytes[smNdx] * pixScale - 0.5f), blueBytes[smNdx] * pixScale - 0.5f)); + row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f)); else - row.Add(new Coord(redBytes[smNdx] * pixScale - 0.5f, greenBytes[smNdx] * pixScale - 0.5f, blueBytes[smNdx] * pixScale - 0.5f)); + row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f)); ++smNdx; } @@ -161,23 +159,39 @@ namespace PrimMesher return rows; } - private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight, - System.Drawing.Drawing2D.InterpolationMode interpMode) + private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight) { - Bitmap scaledImage = new Bitmap(srcImage, destWidth, destHeight); - scaledImage.SetResolution(96.0f, 96.0f); - - Graphics grPhoto = Graphics.FromImage(scaledImage); - grPhoto.InterpolationMode = interpMode; - grPhoto.DrawImage(srcImage, - new Rectangle(0, 0, destWidth, destHeight), - new Rectangle(0, 0, srcImage.Width, srcImage.Height), - GraphicsUnit.Pixel); + Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); + + Color c; + float xscale = srcImage.Width / destWidth; + float yscale = srcImage.Height / destHeight; + + float sy = 0.5f; + for (int y = 0; y < destHeight; y++) + { + float sx = 0.5f; + for (int x = 0; x < destWidth; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), (int)(sy)); + scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } - grPhoto.Dispose(); + sx += xscale; + } + sy += yscale; + } + srcImage.Dispose(); return scaledImage; } + + } + } -} #endif diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index b0dbe50..073c37f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1,4 +1,3 @@ - /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. @@ -1056,9 +1055,8 @@ namespace OpenSim.Region.Physics.OdePlugin return false; } -// primOOBoffset = mesh.GetCentroid(); -// hasOOBoffsetFromMesh = true; - hasOOBoffsetFromMesh = false; + primOOBoffset = mesh.GetCentroid(); + hasOOBoffsetFromMesh = true; _triMeshData = d.GeomTriMeshDataCreate(); -- cgit v1.1 From 33a9f0f1c5aa4cb6b49399dfe6b309b59f77266e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 04:42:45 +0000 Subject: a bit better vertical attractor and banking --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 35 ++++++++++++++-------- 1 file changed, 23 insertions(+), 12 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 0272f34..8999621 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -1,4 +1,3 @@ - /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. @@ -161,7 +160,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularDeflectionTimescale = pValue; break; case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < timestep) pValue = timestep; + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < 0.5f) pValue = 0.5f; else if (pValue > 120) pValue = 120; m_angularMotorDecayTimescale = pValue * invtimestep; break; @@ -210,7 +211,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearDeflectionTimescale = pValue; break; case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - if (pValue < timestep) pValue = timestep; + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < 0.5f) pValue = 0.5f; else if (pValue > 120) pValue = 120; m_linearMotorDecayTimescale = pValue * invtimestep; break; @@ -788,38 +791,46 @@ namespace OpenSim.Region.Physics.OdePlugin } } + float r; + float p; + float y; + rotq.GetEulerAngles(out r, out p, out y); + // vertical atractor if (m_verticalAttractionTimescale < 300) { float roll; float pitch; - GetRollPitch(rotq, out roll, out pitch); + GetRollPitch(irotq, out roll, out pitch); float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; if (Math.Abs(roll) > 0.01) // roll { - torque.X -= roll * ftmp + curLocalAngVel.X * ftmp2; + torque.X -= -roll * ftmp + curLocalAngVel.X * ftmp2; } if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch { - torque.Y -= pitch * ftmp + curLocalAngVel.Y * ftmp2; + torque.Y -= -pitch * ftmp + curLocalAngVel.Y * ftmp2; } - if (m_bankingEfficiency != 0 && Math.Abs(roll) < 0.01) + if (m_bankingEfficiency != 0 && Math.Abs(roll) > 0.01) { - float broll = -roll * m_bankingEfficiency; ; + float broll = roll * m_bankingEfficiency; ; if (m_bankingMix != 0) { - float vfact = m_bankingMix * Math.Abs(curLocalVel.X) / 10.0f; - if (vfact < m_bankingMix) + float vfact = Math.Abs(curLocalVel.X) / 10.0f; + if (vfact > 1.0f) vfact = 1.0f; + if (curLocalVel.X >= 0) broll *= ((1 - m_bankingMix) + vfact); + else + broll *= -((1 - m_bankingMix) + vfact); } - - torque.Z += (broll - curLocalAngVel.Z) / m_bankingTimescale; + broll = (broll - curLocalAngVel.Z) / m_bankingTimescale; + torque.Z += broll; } } -- cgit v1.1 From 3f9c390b4d1e13057b11ef810f19fde7c6722fca Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 14:29:36 +0000 Subject: changes to vehicle banking --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 92 ++++++---------------- 1 file changed, 23 insertions(+), 69 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 8999621..80218e7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -38,6 +38,8 @@ * settings use. */ +// Ubit 2012 + using System; using System.Collections.Generic; using System.Reflection; @@ -57,24 +59,15 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_type; } } -// private OdeScene m_parentScene = null; -// private IntPtr m_aMotor = IntPtr.Zero; - - private OdePrim rootPrim; private OdeScene _pParentScene; - - - - private Vector3 refUpAxis = new Vector3(0, 0, 1); - private Vector3 refAtAxis = new Vector3(1, 0, 0); - - // Vehicle properties + private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind - private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: // HOVER_TERRAIN_ONLY // HOVER_GLOBAL_HEIGHT @@ -84,7 +77,6 @@ namespace OpenSim.Region.Physics.OdePlugin // LIMIT_MOTOR_UP // LIMIT_ROLL_ONLY private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl - private Quaternion m_RollreferenceFrame = Quaternion.Identity; // Linear properties private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time @@ -116,7 +108,6 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_VhoverHeight = 0f; private float m_VhoverEfficiency = 0f; private float m_VhoverTimescale = 1000f; - private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. @@ -126,9 +117,6 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_verticalAttractionEfficiency = 1.0f; // damped private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. - // special contact data for vehicles - public ContactData VehiculeContactData = new ContactData(0f, 0.1f); - // auxiliar private Vector3 m_dir = Vector3.Zero; // velocity applied to body @@ -271,7 +259,6 @@ namespace OpenSim.Region.Physics.OdePlugin float timestep = _pParentScene.ODE_STEPSIZE; switch (pParam) { - case Vehicle.ANGULAR_FRICTION_TIMESCALE: if (pValue.X < timestep) pValue.X = timestep; if (pValue.Y < timestep) pValue.Y = timestep; @@ -575,7 +562,6 @@ namespace OpenSim.Region.Physics.OdePlugin return vec; } - public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) { // assuming rot is normalised @@ -617,23 +603,23 @@ namespace OpenSim.Region.Physics.OdePlugin d.Vector3 dvtmp; Vector3 tmpV; + Vector3 curVel; // velocity in world + Vector3 curAngVel; // angular velocity in world + Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame + Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame + d.Vector3 dtorque = new d.Vector3(); + dvtmp = d.BodyGetLinearVel(Body); - Vector3 curVel; curVel.X = dvtmp.X; curVel.Y = dvtmp.Y; curVel.Z = dvtmp.Z; Vector3 curLocalVel = curVel * irotq; // current velocity in local dvtmp = d.BodyGetAngularVel(Body); - Vector3 curAngVel; curAngVel.X = dvtmp.X; curAngVel.Y = dvtmp.Y; curAngVel.Z = dvtmp.Z; - Vector3 curLocalAngVel = curAngVel * irotq; // current velocity in local - - Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame - Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in object frame - d.Vector3 dtorque = new d.Vector3(); + Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local // linear motor if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) @@ -791,11 +777,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - float r; - float p; - float y; - rotq.GetEulerAngles(out r, out p, out y); - // vertical atractor if (m_verticalAttractionTimescale < 300) { @@ -830,7 +811,17 @@ namespace OpenSim.Region.Physics.OdePlugin broll *= -((1 - m_bankingMix) + vfact); } broll = (broll - curLocalAngVel.Z) / m_bankingTimescale; - torque.Z += broll; + // torque.Z += broll; + + // make z rot be in world Z not local as seems to be in sl + tmpV.X = 0; + tmpV.Y = 0; + tmpV.Z = broll; + tmpV *= irotq; + + torque.X += tmpV.X; + torque.Y += tmpV.Y; + torque.Z += tmpV.Z; } } @@ -853,43 +844,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame } - - //end MoveAngular - - // limit rotations -/* - bool changed = false; - - if (m_RollreferenceFrame != Quaternion.Identity) - { - if (rotq.X >= m_RollreferenceFrame.X) - { - rot.X = rotq.X - (m_RollreferenceFrame.X / 2); - } - if (rotq.Y >= m_RollreferenceFrame.Y) - { - rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2); - } - if (rotq.X <= -m_RollreferenceFrame.X) - { - rot.X = rotq.X + (m_RollreferenceFrame.X / 2); - } - if (rotq.Y <= -m_RollreferenceFrame.Y) - { - rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2); - } - changed = true; - } - - if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0) - { - rot.X = 0; - rot.Y = 0; - changed = true; - } - if (changed) - d.BodySetQuaternion(Body, ref rot); -*/ } } } -- cgit v1.1 From d4e28ed113659fa0e497aa0adedc31d1deddf2ec Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 15:18:13 +0000 Subject: vehicle changes done by simulation thread and not calling one --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 325 ++++++++++++++--------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 7 +- 2 files changed, 200 insertions(+), 132 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 073c37f..53a39d7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -25,7 +25,7 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -/* Revision 2011 by Ubit Umarov +/* Revision 2011/12 by Ubit Umarov * * */ @@ -190,7 +190,7 @@ namespace OpenSim.Region.Physics.OdePlugin public ODEDynamics m_vehicle; internal int m_material = (int)Material.Wood; - protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f}; + protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f }; /// /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. @@ -209,7 +209,7 @@ namespace OpenSim.Region.Physics.OdePlugin AddChange(changes.Physical, value); } } - + public override bool Building // this is not reliable for internal use { get { return m_building; } @@ -225,23 +225,10 @@ namespace OpenSim.Region.Physics.OdePlugin { get { -/* - ODEDynamics v; - if(childPrim && _parent !=null) - { - v =((OdePrim)_parent).m_vehicle; - if(v != null && v.Type != Vehicle.TYPE_NONE) - return v.VehiculeContactData; - return primContactData; - } - - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - return m_vehicle.VehiculeContactData; -*/ - return primContactData; + return primContactData; } } - + public override int PhysicsActorType { get { return (int)ActorTypes.Prim; } @@ -276,11 +263,11 @@ namespace OpenSim.Region.Physics.OdePlugin { set { - if(value) + if (value) m_isSelected = value; AddChange(changes.Selected, value); } - } + } public override bool Flying { @@ -400,10 +387,9 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override void SetVolumeDetect(int param) { - AddChange(changes.VolumeDtc,(param != 0)); + AddChange(changes.VolumeDtc, (param != 0)); } public override Vector3 GeometricCenter @@ -483,14 +469,6 @@ namespace OpenSim.Region.Physics.OdePlugin // client object interpolation works a 'little' better if (_zeroFlag) return Vector3.Zero; -/* - Vector3 returnVelocity = Vector3.Zero; - returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; - returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; - returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; - - return returnVelocity; - */ return _velocity; } set @@ -498,7 +476,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { AddChange(changes.Velocity, value); -// _velocity = value; + // _velocity = value; } else @@ -548,7 +526,7 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - if (givefakeori>0) + if (givefakeori > 0) return fakeori; else @@ -603,7 +581,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - + public override float Buoyancy { get { return m_buoyancy; } @@ -652,6 +630,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override int VehicleType { + // we may need to put a fake on this get { if (m_vehicle == null) @@ -661,44 +640,32 @@ namespace OpenSim.Region.Physics.OdePlugin } set { - if (m_vehicle == null) - { - if (value != (int)Vehicle.TYPE_NONE) - { - m_vehicle = new ODEDynamics(this); - m_vehicle.ProcessTypeChange((Vehicle)value); - } - } - else - m_vehicle.ProcessTypeChange((Vehicle)value); + AddChange(changes.VehicleType, value); } } public override void VehicleFloatParam(int param, float value) { - if (m_vehicle == null) - return; - m_vehicle.ProcessFloatVehicleParam((Vehicle)param, value); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + strVehicleFloatParam fp = new strVehicleFloatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleFloatParam, fp); } public override void VehicleVectorParam(int param, Vector3 value) { - if (m_vehicle == null) - return; - m_vehicle.ProcessVectorVehicleParam((Vehicle)param, value); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + strVehicleVectorParam fp = new strVehicleVectorParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); } - public override void VehicleRotationParam(int param, Quaternion rotation) + public override void VehicleRotationParam(int param, Quaternion value) { - if (m_vehicle == null) - return; - m_vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + strVehicleQuatParam fp = new strVehicleQuatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); } public override void VehicleFlags(int param, bool remove) @@ -829,7 +796,7 @@ namespace OpenSim.Region.Physics.OdePlugin return false; } - + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) { @@ -866,7 +833,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (size.Z <= 0) size.Z = 0.01f; _size = size; - + if (!QuaternionIsFinite(rotation)) { @@ -1023,7 +990,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null) { OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this,false); + parent.ChildDelink(this, false); } } else @@ -1093,23 +1060,23 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.geom_name_map[prim_geom] = Name; _parent_scene.actor_name_map[prim_geom] = this; -/* - if (childPrim) - { - if (_parent != null && _parent is OdePrim) - { - OdePrim parent = (OdePrim)_parent; - //Console.WriteLine("SetGeom calls ChildSetGeom"); - parent.ChildSetGeom(this); - } - } - */ + /* + if (childPrim) + { + if (_parent != null && _parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; + //Console.WriteLine("SetGeom calls ChildSetGeom"); + parent.ChildSetGeom(this); + } + } + */ } else m_log.Warn("Setting bad Geom"); } - + /// /// Create a geometry for the given mesh in the given target space. /// @@ -1128,10 +1095,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent_scene.needsMeshing(_pbs)) { - haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims } - if(!haveMesh) + if (!haveMesh) { if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 && _size.X == _size.Y && _size.Y == _size.Z) @@ -1186,7 +1153,7 @@ namespace OpenSim.Region.Physics.OdePlugin // catch (System.AccessViolationException) catch (Exception e) { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name,e); + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); } prim_geom = IntPtr.Zero; @@ -1526,7 +1493,7 @@ namespace OpenSim.Region.Physics.OdePlugin } #region Mass Calculation - + private float CalculatePrimVolume() { float volume = _size.X * _size.Y * _size.Z; // default @@ -1885,7 +1852,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prm.prim_geom != IntPtr.Zero) d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - if(prm.Body != prim.Body) + if (prm.Body != prim.Body) prm.DestroyBody(); // don't loose bodies around prm.Body = IntPtr.Zero; } @@ -2057,21 +2024,21 @@ namespace OpenSim.Region.Physics.OdePlugin SetInStaticSpace(this); } -// m_building = false; // REMOVE THIS LATER + // m_building = false; // REMOVE THIS LATER if (m_isphysical && Body == IntPtr.Zero) { -/* - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - { - */ - MakeBody(); -// } + /* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + { + */ + MakeBody(); + // } } } @@ -2114,7 +2081,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (NewParent != _parent) { - (_parent as OdePrim).ChildDelink(this,false); // for now... + (_parent as OdePrim).ChildDelink(this, false); // for now... childPrim = false; if (NewParent != null) @@ -2130,7 +2097,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void Stop() { - if(!childPrim) + if (!childPrim) { m_force = Vector3.Zero; m_forceacc = Vector3.Zero; @@ -2138,7 +2105,7 @@ namespace OpenSim.Region.Physics.OdePlugin _torque = Vector3.Zero; _velocity = Vector3.Zero; _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; _target_velocity = Vector3.Zero; if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Stop(); @@ -2150,7 +2117,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetTorque(Body, 0f, 0f, 0f); d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetAngularVel(Body, 0f, 0f, 0f); - + } } @@ -2236,7 +2203,7 @@ namespace OpenSim.Region.Physics.OdePlugin givefakepos--; if (givefakepos < 0) givefakepos = 0; -// changeSelectedStatus(); + // changeSelectedStatus(); resetCollisionAccounting(); } @@ -2381,17 +2348,17 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim) { - if (NewStatus) + if (NewStatus) { if (Body == IntPtr.Zero) { -/* - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - */ + /* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ { MakeBody(); } @@ -2402,13 +2369,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) { // UpdateChildsfromgeom(); -/* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - */ - DestroyBody(); + /* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ + DestroyBody(); } } } @@ -2417,7 +2384,7 @@ namespace OpenSim.Region.Physics.OdePlugin } private void changeprimsizeshape() - { + { OdePrim parent = (OdePrim)_parent; bool chp = childPrim; @@ -2609,6 +2576,57 @@ namespace OpenSim.Region.Physics.OdePlugin } } + private void changeVehicleType(int value) + { + if (m_vehicle == null) + { + if (value != (int)Vehicle.TYPE_NONE) + { + m_vehicle = new ODEDynamics(this); + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + else + m_vehicle.ProcessTypeChange((Vehicle)value); + } + + private void changeVehicleFloatParam(strVehicleFloatParam fp) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleVectorParam(strVehicleVectorParam vp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleRotationParam(strVehicleQuatParam qp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleFlags(strVehicleBoolParam bp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + #endregion public void Move() @@ -2616,10 +2634,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim && m_isphysical && Body != IntPtr.Zero && !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building) // KF: Only move root prims. { -// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + // if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 float timestep = _parent_scene.ODE_STEPSIZE; - + float fx = 0; float fy = 0; float fz = 0; @@ -2631,13 +2649,13 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - float m_mass = _mass; - + float m_mass = _mass; + // fz = 0f; //m_log.Info(m_collisionFlags.ToString()); if (m_usePID) { - + // If the PID Controller isn't active then we set our force // calculating base velocity to the current position @@ -2795,7 +2813,7 @@ namespace OpenSim.Region.Physics.OdePlugin fz += m_forceacc.Z; m_forceacc = Vector3.Zero; - + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); if (fx != 0 || fy != 0 || fz != 0) { @@ -2871,10 +2889,10 @@ namespace OpenSim.Region.Physics.OdePlugin bool lastZeroFlag = _zeroFlag; d.Vector3 lpos; - d.GeomCopyPosition(prim_geom,out lpos); // root position that is seem by rest of simulator - + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + // we need to use root position since that's all the rest of scene uses - if ( lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X + if (lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y ) { @@ -3137,10 +3155,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null) { OdePrim parent = (OdePrim)_parent; - parent.ChildRemove(this,false); + parent.ChildRemove(this, false); } else - ChildRemove(this,false); + ChildRemove(this, false); RemoveGeom(); m_targetSpace = IntPtr.Zero; @@ -3177,12 +3195,12 @@ namespace OpenSim.Region.Physics.OdePlugin changevelocity((Vector3)arg); break; -// case changes.Acceleration: -// changeacceleration((Vector3)arg); -// break; -// case changes.AngVelocity: -// changeangvelocity((Vector3)arg); -// break; + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; case changes.Force: changeForce((Vector3)arg); @@ -3209,7 +3227,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; case changes.Shape: - changeShape((PrimitiveBaseShape) arg); + changeShape((PrimitiveBaseShape)arg); break; case changes.CollidesWater: @@ -3229,13 +3247,33 @@ namespace OpenSim.Region.Physics.OdePlugin break; case changes.disabled: - changeDisable((bool) arg); + changeDisable((bool)arg); break; case changes.building: changeBuilding((bool)arg); break; + case changes.VehicleType: + changeVehicleType((int)arg); + break; + + case changes.VehicleFlags: + changeVehicleFlags((strVehicleBoolParam) arg); + break; + + case changes.VehicleFloatParam: + changeVehicleFloatParam((strVehicleFloatParam) arg); + break; + + case changes.VehicleVectorParam: + changeVehicleVectorParam((strVehicleVectorParam) arg); + break; + + case changes.VehicleRotationParam: + changeVehicleRotationParam((strVehicleQuatParam) arg); + break; + case changes.Null: donullchange(); break; @@ -3251,5 +3289,30 @@ namespace OpenSim.Region.Physics.OdePlugin { _parent_scene.AddChange(this, what, arg); } + + + private struct strVehicleBoolParam + { + public int param; + public bool value; + } + + private struct strVehicleFloatParam + { + public int param; + public float value; + } + + private struct strVehicleQuatParam + { + public int param; + public Quaternion value; + } + + private struct strVehicleVectorParam + { + public int param; + public Vector3 value; + } } -} +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index fc59c32..86385bf 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1,4 +1,3 @@ - /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. @@ -137,6 +136,12 @@ namespace OpenSim.Region.Physics.OdePlugin disabled, building, + VehicleType, + VehicleFloatParam, + VehicleVectorParam, + VehicleRotationParam, + VehicleFlags, + Null //keep this last used do dim the methods array. does nothing but pulsing the prim } -- cgit v1.1 From 022ae33ed5da0b1d9b56694bcfbf2f1a9e18b36e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 17:35:38 +0000 Subject: UbitODE: replace 'taints' by 'changes' for avatars also. This provides better time order with changes in prims. --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 563 +++++++++++++-------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 14 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 51 +- 3 files changed, 378 insertions(+), 250 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b0b91f6..cf7fdca 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -26,7 +26,7 @@ */ -// Revision by Ubit 2011 +// Revision by Ubit 2011/12 using System; using System.Collections.Generic; @@ -95,21 +95,12 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_iscollidingObj = false; private bool m_alwaysRun = false; private int m_requestedUpdateFrequency = 0; - private Vector3 m_taintPosition = Vector3.Zero; - private bool m_hasTaintPosition = false; public uint m_localID = 0; public bool m_returnCollisions = false; // taints and their non-tainted counterparts public bool m_isPhysical = false; // the current physical status - public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing) public float MinimumGroundFlightOffset = 3f; - - private Vector3 m_taintForce; - private bool m_hasTaintForce; - private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. - - private float m_buoyancy = 0f; // private CollisionLocker ode; @@ -151,15 +142,13 @@ namespace OpenSim.Region.Physics.OdePlugin { m_uuid = UUID.Random(); - m_hasTaintPosition = false; - if (pos.IsFinite()) { - if (pos.Z > 9999999f) + if (pos.Z > 99999f) { pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } - if (pos.Z < -90000f) + if (pos.Z < -100f) // shouldn't this be 0 ? { pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } @@ -187,15 +176,12 @@ namespace OpenSim.Region.Physics.OdePlugin CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f; //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); - m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH; m_isPhysical = false; // current status: no ODE information exists - m_tainted_isPhysical = true; // new tainted status: need to create ODE information - - m_hasTaintForce = false; - _parent_scene.AddPhysicsActorTaint(this); m_name = avName; + + AddChange(changes.Add, null); } public override int PhysicsActorType @@ -402,16 +388,11 @@ namespace OpenSim.Region.Physics.OdePlugin { value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } - if (value.Z < -90000f) + if (value.Z < -100f) { value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } - - m_taintPosition.X = value.X; - m_taintPosition.Y = value.Y; - m_taintPosition.Z = value.Z; - m_hasTaintPosition = true; - _parent_scene.AddPhysicsActorTaint(this); + AddChange(changes.Position, value); } else { @@ -440,11 +421,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (value.IsFinite()) { - m_pidControllerActive = true; - - Vector3 SetSize = value; - m_tainted_CAPSULE_LENGTH = SetSize.Z *1.15f - CAPSULE_RADIUS * 2.0f; - _parent_scene.AddPhysicsActorTaint(this); + AddChange(changes.Size, value); } else { @@ -460,129 +437,6 @@ namespace OpenSim.Region.Physics.OdePlugin /// /// - // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access - // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only - // place that is safe to call this routine AvatarGeomAndBodyCreation. - private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) - { - _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); - if (CAPSULE_LENGTH <= 0) - { - m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); - CAPSULE_LENGTH = 0.01f; - - } - - if (CAPSULE_RADIUS <= 0) - { - m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); - CAPSULE_RADIUS = 0.01f; - - } - Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); - - d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); - d.GeomSetCollideBits(Shell, (int)m_collisionFlags); - - d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); - - m_mass = ShellMass.mass; // update mass - - // rescale PID parameters - PID_D = _parent_scene.avPIDD; - PID_P = _parent_scene.avPIDP; - - // rescale PID parameters so that this aren't affected by mass - // and so don't get unstable for some masses - // also scale by ode time step so you don't need to refix them - - PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps - PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; - PID_P /= 50 * 80; - PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; - - Body = d.BodyCreate(_parent_scene.world); - - d.BodySetAutoDisableFlag(Body, false); - d.BodySetPosition(Body, npositionX, npositionY, npositionZ); - - _position.X = npositionX; - _position.Y = npositionY; - _position.Z = npositionZ; - - m_hasTaintPosition = false; - - d.BodySetMass(Body, ref ShellMass); - d.GeomSetBody(Shell, Body); - - // The purpose of the AMotor here is to keep the avatar's physical - // surrogate from rotating while moving - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - - d.JointSetAMotorMode(Amotor, 0); - d.JointSetAMotorNumAxes(Amotor, 3); - d.JointSetAMotorAxis(Amotor, 0, 0 , 1, 0, 0); - d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); - d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); - - d.JointSetAMotorAngle(Amotor, 0, 0); - d.JointSetAMotorAngle(Amotor, 1, 0); - d.JointSetAMotorAngle(Amotor, 2, 0); - - d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD - d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); - d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); - d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); - - // These lowstops and high stops are effectively (no wiggle room) - d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); - - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); - - d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f); - } - - /// - /// Destroys the avatar body and geom - - private void AvatarGeomAndBodyDestroy() - { - // Kill the Amotor - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - - if (Body != IntPtr.Zero) - { - //kill the body - d.BodyDestroy(Body); - Body = IntPtr.Zero; - } - - //kill the Geometry - if (Shell != IntPtr.Zero) - { - _parent_scene.geom_name_map.Remove(Shell); - _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); - d.GeomDestroy(Shell); - _parent_scene.geom_name_map.Remove(Shell); - Shell = IntPtr.Zero; - } - } // /// /// Uses the capped cyllinder volume formula to calculate the avatar's mass. @@ -705,8 +559,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (value.IsFinite()) { - m_pidControllerActive = true; - _target_velocity = value; + AddChange(changes.Velocity, value); } else { @@ -738,9 +591,6 @@ namespace OpenSim.Region.Physics.OdePlugin get { return Quaternion.Identity; } set { - //Matrix3 or = Orientation.ToRotationMatrix(); - //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22); - //d.BodySetRotation(Body, ref ord); } } @@ -767,18 +617,11 @@ namespace OpenSim.Region.Physics.OdePlugin { if (pushforce) { - m_pidControllerActive = false; - // scale with odetime step and density - m_taintForce = force * m_density / _parent_scene.ODE_STEPSIZE / 28f; - m_hasTaintForce = true; - _parent_scene.AddPhysicsActorTaint(this); + AddChange(changes.Force, force * m_density / _parent_scene.ODE_STEPSIZE / 28f); } else { - m_pidControllerActive = true; - _target_velocity.X += force.X; - _target_velocity.Y += force.Y; - _target_velocity.Z += force.Z; + AddChange(changes.Velocity, force); } } else @@ -798,6 +641,128 @@ namespace OpenSim.Region.Physics.OdePlugin } + // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access + // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only + // place that is safe to call this routine AvatarGeomAndBodyCreation. + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) + { + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + if (CAPSULE_LENGTH <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_LENGTH = 0.01f; + + } + + if (CAPSULE_RADIUS <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_RADIUS = 0.01f; + + } + Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); + + d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); + d.GeomSetCollideBits(Shell, (int)m_collisionFlags); + + d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); + + m_mass = ShellMass.mass; // update mass + + // rescale PID parameters + PID_D = _parent_scene.avPIDD; + PID_P = _parent_scene.avPIDP; + + // rescale PID parameters so that this aren't affected by mass + // and so don't get unstable for some masses + // also scale by ode time step so you don't need to refix them + + PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps + PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; + PID_P /= 50 * 80; + PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; + + Body = d.BodyCreate(_parent_scene.world); + + d.BodySetAutoDisableFlag(Body, false); + d.BodySetPosition(Body, npositionX, npositionY, npositionZ); + + _position.X = npositionX; + _position.Y = npositionY; + _position.Z = npositionZ; + + d.BodySetMass(Body, ref ShellMass); + d.GeomSetBody(Shell, Body); + + // The purpose of the AMotor here is to keep the avatar's physical + // surrogate from rotating while moving + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + d.JointSetAMotorNumAxes(Amotor, 3); + d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); + d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); + d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); + + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorAngle(Amotor, 1, 0); + d.JointSetAMotorAngle(Amotor, 2, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); + + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f); + } + + /// + /// Destroys the avatar body and geom + + private void AvatarGeomAndBodyDestroy() + { + // Kill the Amotor + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + Body = IntPtr.Zero; + } + + //kill the Geometry + if (Shell != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(Shell); + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } + } + /// /// Called from Simulate /// This is the avatar's movement control + PID Controller @@ -1136,8 +1101,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// public void Destroy() { - m_tainted_isPhysical = false; - _parent_scene.AddPhysicsActorTaint(this); + AddChange(changes.Remove, null); } public override void CrossingFailure() @@ -1207,12 +1171,11 @@ namespace OpenSim.Region.Physics.OdePlugin return m_haseventsubscription; } - public void ProcessTaints(float timestep) + private void changePhysicsStatus(bool NewStatus) { - - if (m_tainted_isPhysical != m_isPhysical) + if (NewStatus != m_isPhysical) { - if (m_tainted_isPhysical) + if (NewStatus) { // Create avatar capsule and related ODE data if ((Shell != IntPtr.Zero)) @@ -1237,60 +1200,246 @@ namespace OpenSim.Region.Physics.OdePlugin AvatarGeomAndBodyDestroy(); } - m_isPhysical = m_tainted_isPhysical; + m_isPhysical = NewStatus; } + } + + private void changeAdd() + { + changePhysicsStatus(true); + } - if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) + private void changeRemove() + { + changePhysicsStatus(false); + } + + private void changeShape(PrimitiveBaseShape arg) + { + } + + private void changeSize(Vector3 Size) + { + if (Size.IsFinite()) { - if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + float caplen = Size.Z; + + caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f; + + if (caplen != CAPSULE_LENGTH) { - AvatarGeomAndBodyDestroy(); + if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + { + AvatarGeomAndBodyDestroy(); - m_pidControllerActive = true; + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = caplen; - float prevCapsule = CAPSULE_LENGTH; - CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; - - AvatarGeomAndBodyCreation(_position.X, _position.Y, - _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); - Velocity = Vector3.Zero; + Velocity = Vector3.Zero; - _parent_scene.geom_name_map[Shell] = m_name; - _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; - } - else - { - m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " - + (Shell == IntPtr.Zero ? "Shell " : "") - + (Body == IntPtr.Zero ? "Body " : "") - + (Amotor == IntPtr.Zero ? "Amotor " : "")); + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell == IntPtr.Zero ? "Shell " : "") + + (Body == IntPtr.Zero ? "Body " : "") + + (Amotor == IntPtr.Zero ? "Amotor " : "")); + } } + + m_pidControllerActive = true; + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); } + } - if (m_hasTaintPosition) + private void changePosition( Vector3 newPos) { if (Body != IntPtr.Zero) - d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); + d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + + private void changeOrientation(Quaternion newOri) + { + } + + private void changeVelocity(Vector3 newVel) + { + m_pidControllerActive = true; + _target_velocity = newVel; + } - _position.X = m_taintPosition.X; - _position.Y = m_taintPosition.Y; - _position.Z = m_taintPosition.Z; - m_hasTaintPosition = false; + private void changeSetTorque(Vector3 newTorque) + { + } + + private void changeAddForce(Vector3 newForce) + { + } + + private void changeAddAngularForce(Vector3 arg) + { + } + + private void changeAngularLock(Vector3 arg) + { + } + + private void changeFloatOnWater(bool arg) + { + } + + private void changeVolumedetetion(bool arg) + { + } + + private void changeSelectedStatus(bool arg) + { + } + + private void changeDisable(bool arg) + { + } + + private void changeBuilding(bool arg) + { + } + + private void changeForce(Vector3 newForce) + { + m_pidControllerActive = false; + if (Body != IntPtr.Zero) + { + if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) + d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); + } + } + + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; } - if (m_hasTaintForce) + // nasty switch + switch (what) { - if (Body != IntPtr.Zero) - { - if(m_taintForce.X !=0f || m_taintForce.Y !=0f || m_taintForce.Z !=0) - d.BodyAddForce(Body, m_taintForce.X, m_taintForce.Y, m_taintForce.Z); - m_hasTaintForce = false; - } + case changes.Add: + changeAdd(); + break; + case changes.Remove: + changeRemove(); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changeVelocity((Vector3)arg); + break; + + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; +/* not in use for now + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; +*/ + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; } + return false; + } + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor)this, what, arg); } + internal void AddCollisionFrameTime(int p) { // protect it from overflow crashing diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 53a39d7..078adeb 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -668,13 +668,14 @@ namespace OpenSim.Region.Physics.OdePlugin AddChange(changes.VehicleVectorParam, fp); } - public override void VehicleFlags(int param, bool remove) + public override void VehicleFlags(int param, bool value) { if (m_vehicle == null) return; - m_vehicle.ProcessVehicleFlags(param, remove); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); } public void SetAcceleration(Vector3 accel) @@ -2374,8 +2375,9 @@ namespace OpenSim.Region.Physics.OdePlugin changeShape(_pbs); } else - */ + */ DestroyBody(); + Stop(); } } } @@ -3287,7 +3289,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void AddChange(changes what, object arg) { - _parent_scene.AddChange(this, what, arg); + _parent_scene.AddChange((PhysicsActor) this, what, arg); } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 86385bf..2dfcca6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -147,7 +147,7 @@ namespace OpenSim.Region.Physics.OdePlugin public struct ODEchangeitem { - public OdePrim prim; + public PhysicsActor actor; public OdeCharacter character; public changes what; public Object arg; @@ -187,7 +187,6 @@ namespace OpenSim.Region.Physics.OdePlugin public float gravityy = 0f; public float gravityz = -9.8f; - private float waterlevel = 0f; private int framecount = 0; @@ -1558,23 +1557,15 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } - public void AddChange(OdePrim prim, changes what, Object arg) - { - ODEchangeitem item = new ODEchangeitem(); - item.prim = prim; - item.what = what; - item.arg = arg; - ChangesQueue.Enqueue(item); - } - /// - /// Called to queue a change to a prim + /// Called to queue a change to a actor /// to use in place of old taint mechanism so changes do have a time sequence /// - public void AddChange(OdeCharacter character, changes what, Object arg) + + public void AddChange(PhysicsActor actor, changes what, Object arg) { ODEchangeitem item = new ODEchangeitem(); - item.character = character; + item.actor = actor; item.what = what; item.arg = arg; ChangesQueue.Enqueue(item); @@ -1687,25 +1678,6 @@ namespace OpenSim.Region.Physics.OdePlugin // clear pointer/counter to contacts to pass into joints m_global_contactcount = 0; - // do characters requested changes - - OdeCharacter character; - int numtaints; - lock (_taintedCharacterLock) - { - numtaints = _taintedCharacterQ.Count; - // if (numtaints > 50) - // numtaints = 50; - while (numtaints > 0) - { - character = _taintedCharacterQ.Dequeue(); - character.ProcessTaints(ODE_STEPSIZE); - _taintedCharacterH.Remove(character); - numtaints--; - } - } - // do other objects requested changes - ODEchangeitem item; if(ChangesQueue.Count >0) @@ -1716,14 +1688,19 @@ namespace OpenSim.Region.Physics.OdePlugin while(ChangesQueue.Dequeue(out item)) { - if (item.prim != null) + if (item.actor != null) { try { - if (item.prim.DoAChange(item.what, item.arg)) - RemovePrimThreadLocked(item.prim); + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); } - catch { }; + catch + { + m_log.Warn("[PHYSICS]: doChange failed for a actor"); + }; } ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); if (ttmp > 20) -- cgit v1.1 From bcf59a574f757fd8a79c3fc3bb61fab87da8c1f1 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 19:12:04 +0000 Subject: remove forgotten taints code --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 32 ------------------------ 1 file changed, 32 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 2dfcca6..233267e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -228,10 +228,6 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly HashSet _prims = new HashSet(); private readonly HashSet _activeprims = new HashSet(); - private readonly Object _taintedCharacterLock = new Object(); - private readonly HashSet _taintedCharacterH = new HashSet(); // faster verification of repeated character taints - private readonly Queue _taintedCharacterQ = new Queue(); // character taints - public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); /// @@ -1579,34 +1575,6 @@ namespace OpenSim.Region.Physics.OdePlugin /// public override void AddPhysicsActorTaint(PhysicsActor prim) { - if (prim is OdePrim) - { -/* OdePrim taintedprim = ((OdePrim) prim); - lock (_taintedPrimLock) - { - if (!(_taintedPrimH.Contains(taintedprim))) - { - _taintedPrimH.Add(taintedprim); // HashSet for searching - _taintedPrimQ.Enqueue(taintedprim); // List for ordered readout - } - } - */ - return; - } - else if (prim is OdeCharacter) - { - OdeCharacter taintedchar = ((OdeCharacter)prim); - lock (_taintedCharacterLock) - { - if (!(_taintedCharacterH.Contains(taintedchar))) - { - _taintedCharacterH.Add(taintedchar); - _taintedCharacterQ.Enqueue(taintedchar); - if (taintedchar.bad) - m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid); - } - } - } } /// -- cgit v1.1 From f168fefb790e1c951a7a59cafa4d2cb2520e4e06 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 19:14:27 +0000 Subject: removed outdated coments. --- .../Physics/UbitOdePlugin/ODEDynamics.c_comments | 630 --------------------- 1 file changed, 630 deletions(-) delete mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments deleted file mode 100644 index 1060aa6..0000000 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments +++ /dev/null @@ -1,630 +0,0 @@ -/* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - * - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using log4net; -using OpenMetaverse; -using Ode.NET; -using OpenSim.Framework; -using OpenSim.Region.Physics.Manager; - -namespace OpenSim.Region.Physics.OdePlugin -{ - public class ODEDynamics - { - public Vehicle Type - { - get { return m_type; } - } - - public IntPtr Body - { - get { return m_body; } - } - - private int frcount = 0; // Used to limit dynamics debug output to - // every 100th frame - - // private OdeScene m_parentScene = null; - private IntPtr m_body = IntPtr.Zero; - private IntPtr m_jointGroup = IntPtr.Zero; - private IntPtr m_aMotor = IntPtr.Zero; - - - // Vehicle properties - private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind - // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier - private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: - // HOVER_TERRAIN_ONLY - // HOVER_GLOBAL_HEIGHT - // NO_DEFLECTION_UP - // HOVER_WATER_ONLY - // HOVER_UP_ONLY - // LIMIT_MOTOR_UP - // LIMIT_ROLL_ONLY - - // Linear properties - private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time - private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL - private Vector3 m_dir = Vector3.Zero; // velocity applied to body - private Vector3 m_linearFrictionTimescale = Vector3.Zero; - private float m_linearMotorDecayTimescale = 0; - private float m_linearMotorTimescale = 0; - private Vector3 m_lastLinearVelocityVector = Vector3.Zero; - // private bool m_LinearMotorSetLastFrame = false; - // private Vector3 m_linearMotorOffset = Vector3.Zero; - - //Angular properties - private Vector3 m_angularMotorDirection = Vector3.Zero; - private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; - private Vector3 m_angularFrictionTimescale = Vector3.Zero; - private float m_angularMotorDecayTimescale = 0; - private float m_angularMotorTimescale = 0; - private Vector3 m_lastAngularVelocityVector = Vector3.Zero; - - //Deflection properties - // private float m_angularDeflectionEfficiency = 0; - // private float m_angularDeflectionTimescale = 0; - // private float m_linearDeflectionEfficiency = 0; - // private float m_linearDeflectionTimescale = 0; - - //Banking properties - // private float m_bankingEfficiency = 0; - // private float m_bankingMix = 0; - // private float m_bankingTimescale = 0; - - //Hover and Buoyancy properties - private float m_VhoverHeight = 0f; - private float m_VhoverEfficiency = 0f; - private float m_VhoverTimescale = 0f; - private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height - private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. - // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) - // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. - // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. - - //Attractor properties - private float m_verticalAttractionEfficiency = 0; - private float m_verticalAttractionTimescale = 0; - - - - - - internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) - { - switch (pParam) - { - case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_angularDeflectionEfficiency = pValue; - break; - case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_angularDeflectionTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_angularMotorDecayTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_angularMotorTimescale = pValue; - break; - case Vehicle.BANKING_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingEfficiency = pValue; - break; - case Vehicle.BANKING_MIX: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingMix = pValue; - break; - case Vehicle.BANKING_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_bankingTimescale = pValue; - break; - case Vehicle.BUOYANCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; - m_VehicleBuoyancy = pValue; - break; - case Vehicle.HOVER_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - m_VhoverEfficiency = pValue; - break; - case Vehicle.HOVER_HEIGHT: - m_VhoverHeight = pValue; - break; - case Vehicle.HOVER_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_VhoverTimescale = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; - // m_linearDeflectionEfficiency = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - // m_linearDeflectionTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_linearMotorDecayTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_linearMotorTimescale = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: - if (pValue < 0.0f) pValue = 0.0f; - if (pValue > 1.0f) pValue = 1.0f; - m_verticalAttractionEfficiency = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: - if (pValue < 0.01f) pValue = 0.01f; - m_verticalAttractionTimescale = pValue; - break; - - // These are vector properties but the engine lets you use a single float value to - // set all of the components to the same value - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue, pValue, pValue); - m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue, pValue, pValue); - m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); - break; - - } - - }//end ProcessFloatVehicleParam - - internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) - { - switch (pParam) - { - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - } - - }//end ProcessVectorVehicleParam - - internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) - { - switch (pParam) - { - case Vehicle.REFERENCE_FRAME: - // m_referenceFrame = pValue; - break; - } - - }//end ProcessRotationVehicleParam - - internal void ProcessTypeChange(Vehicle pType) - { -Console.WriteLine("ProcessTypeChange to " + pType); - - // Set Defaults For Type - m_type = pType; - switch (pType) - { - case Vehicle.TYPE_SLED: - m_linearFrictionTimescale = new Vector3(30, 1, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 120; - m_VhoverHeight = 0; - m_VhoverEfficiency = 1; - m_VhoverTimescale = 10; - m_VehicleBuoyancy = 0; - // m_linearDeflectionEfficiency = 1; - // m_linearDeflectionTimescale = 1; - // m_angularDeflectionEfficiency = 1; - // m_angularDeflectionTimescale = 1000; - // m_bankingEfficiency = 0; - // m_bankingMix = 1; - // m_bankingTimescale = 10; - // m_referenceFrame = Quaternion.Identity; - m_flags &= - ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_CAR: - m_linearFrictionTimescale = new Vector3(100, 2, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 1; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 1; - m_angularMotorDecayTimescale = 0.8f; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - // // m_linearDeflectionEfficiency = 1; - // // m_linearDeflectionTimescale = 2; - // // m_angularDeflectionEfficiency = 0; - // m_angularDeflectionTimescale = 10; - m_verticalAttractionEfficiency = 1; - m_verticalAttractionTimescale = 10; - // m_bankingEfficiency = -0.2f; - // m_bankingMix = 1; - // m_bankingTimescale = 1; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_BOAT: - m_linearFrictionTimescale = new Vector3(10, 3, 2); - m_angularFrictionTimescale = new Vector3(10,10,10); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 2; - m_VehicleBuoyancy = 1; - // m_linearDeflectionEfficiency = 0.5f; - // m_linearDeflectionTimescale = 3; - // m_angularDeflectionEfficiency = 0.5f; - // m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 0.5f; - m_verticalAttractionTimescale = 5; - // m_bankingEfficiency = -0.3f; - // m_bankingMix = 0.8f; - // m_bankingTimescale = 1; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | - VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_AIRPLANE: - m_linearFrictionTimescale = new Vector3(200, 10, 5); - m_angularFrictionTimescale = new Vector3(20, 20, 20); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 2; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - // m_linearDeflectionEfficiency = 0.5f; - // m_linearDeflectionTimescale = 3; - // m_angularDeflectionEfficiency = 1; - // m_angularDeflectionTimescale = 2; - m_verticalAttractionEfficiency = 0.9f; - m_verticalAttractionTimescale = 2; - // m_bankingEfficiency = 1; - // m_bankingMix = 0.7f; - // m_bankingTimescale = 2; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); - break; - case Vehicle.TYPE_BALLOON: - m_linearFrictionTimescale = new Vector3(5, 5, 5); - m_angularFrictionTimescale = new Vector3(10, 10, 10); - m_linearMotorDirection = Vector3.Zero; - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60; - m_angularMotorDirection = Vector3.Zero; - m_angularMotorTimescale = 6; - m_angularMotorDecayTimescale = 10; - m_VhoverHeight = 5; - m_VhoverEfficiency = 0.8f; - m_VhoverTimescale = 10; - m_VehicleBuoyancy = 1; - // m_linearDeflectionEfficiency = 0; - // m_linearDeflectionTimescale = 5; - // m_angularDeflectionEfficiency = 0; - // m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 1; - m_verticalAttractionTimescale = 1000; - // m_bankingEfficiency = 0; - // m_bankingMix = 0.7f; - // m_bankingTimescale = 5; - // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - break; - - } - }//end SetDefaultsForType - - internal void Enable(IntPtr pBody, OdeScene pParentScene) - { -//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy); - if (m_type == Vehicle.TYPE_NONE) - return; - - m_body = pBody; - //KF: This used to set up the linear and angular joints - } - - internal void Step(float pTimestep, OdeScene pParentScene) - { - if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) - return; - frcount++; // used to limit debug comment output - if (frcount > 100) - frcount = 0; - - MoveLinear(pTimestep, pParentScene); - MoveAngular(pTimestep); - }// end Step - - private void MoveLinear(float pTimestep, OdeScene _pParentScene) - { - if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - - // add drive to body - Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); - m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? - - // This will work temporarily, but we really need to compare speed on an axis - // KF: Limit body velocity to applied velocity? - if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) - m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; - if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) - m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; - if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) - m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; - - // decay applied velocity - Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); - //Console.WriteLine("decay: " + decayfraction); - m_linearMotorDirection -= m_linearMotorDirection * decayfraction; - //Console.WriteLine("actual: " + m_linearMotorDirection); - } - else - { // requested is not significant - // if what remains of applied is small, zero it. - if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) - m_lastLinearVelocityVector = Vector3.Zero; - } - - - // convert requested object velocity to world-referenced vector - m_dir = m_lastLinearVelocityVector; - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - m_dir *= rotq; // apply obj rotation to velocity vector - - // add Gravity andBuoyancy - // KF: So far I have found no good method to combine a script-requested - // .Z velocity and gravity. Therefore only 0g will used script-requested - // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. - Vector3 grav = Vector3.Zero; - if(m_VehicleBuoyancy < 1.0f) - { - // There is some gravity, make a gravity force vector - // that is applied after object velocity. - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; - grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); - // Preserve the current Z velocity - d.Vector3 vel_now = d.BodyGetLinearVel(Body); - m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity - } // else its 1.0, no gravity. - - // Check if hovering - if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) - { - // We should hover, get the target height - d.Vector3 pos = d.BodyGetPosition(Body); - if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) - { - m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) - { - m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) - { - m_VhoverTargetHeight = m_VhoverHeight; - } - - if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) - { - // If body is aready heigher, use its height as target height - if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; - } - -// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped -// m_VhoverTimescale = 0f; // time to acheive height -// pTimestep is time since last frame,in secs - float herr0 = pos.Z - m_VhoverTargetHeight; -//if(frcount == 0) Console.WriteLine("herr0=" + herr0); - // Replace Vertical speed with correction figure if significant - if(Math.Abs(herr0) > 0.01f ) - { - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); - // m_VhoverEfficiency is not yet implemented - } - else - { - m_dir.Z = 0f; - } - } - - // Apply velocity - d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); -//if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z); - // apply gravity force - d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); -//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z); - - - // apply friction - Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); - m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; - } // end MoveLinear() - - private void MoveAngular(float pTimestep) - { - - // m_angularMotorDirection is the latest value from the script, and is decayed here - // m_angularMotorDirectionLASTSET is the latest value from the script - // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here - - if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - // ramp up to new value - Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); - m_lastAngularVelocityVector += (addAmount * 10f); -//if(frcount == 0) Console.WriteLine("add: " + addAmount); - - // limit applied value to what was set by script - // This will work temporarily, but we really need to compare speed on an axis - if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X)) - m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; - if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y)) - m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; - if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) - m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; - - // decay the requested value - Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); - //Console.WriteLine("decay: " + decayfraction); - m_angularMotorDirection -= m_angularMotorDirection * decayfraction; - //Console.WriteLine("actual: " + m_linearMotorDirection); - } - // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ? - - // Vertical attractor section - -// d.Mass objMass; -// d.BodyGetMass(Body, out objMass); -// float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); - float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); - // get present body rotation - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - // make a vector pointing up - Vector3 verterr = Vector3.Zero; - verterr.Z = 1.0f; - // rotate it to Body Angle - verterr = verterr * rotq; - // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. - // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go - // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. - if (verterr.Z < 0.0f) - { - verterr.X = 2.0f - verterr.X; - verterr.Y = 2.0f - verterr.Y; - } - // Error is 0 (no error) to +/- 2 (max error) - // scale it by servo - verterr = verterr * servo; - - // rotate to object frame - // verterr = verterr * rotq; - - // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so - // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. - m_lastAngularVelocityVector.X += verterr.Y; - m_lastAngularVelocityVector.Y -= verterr.X; -/* -if(frcount == 0) - { -// Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector); - Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}", - Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency)); - } - */ - d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z); - // apply friction - Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); - m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; - - } //end MoveAngular - } -} -- cgit v1.1 From 6fd6919a0bcdfd2d15d0b7a7aa392837de759114 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 19:25:41 +0000 Subject: remove drawstuff from ubitode --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 3 - OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 153 --------------------- OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs | 99 ------------- 4 files changed, 1 insertion(+), 256 deletions(-) delete mode 100644 OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index cf7fdca..793e281 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -126,7 +126,7 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr Shell = IntPtr.Zero; public IntPtr Amotor = IntPtr.Zero; public d.Mass ShellMass; - public bool collidelock = false; +// public bool collidelock = false; private bool m_haseventsubscription = false; public int m_eventsubscription = 0; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 078adeb..3b7f562 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2025,9 +2025,6 @@ namespace OpenSim.Region.Physics.OdePlugin SetInStaticSpace(this); } - // m_building = false; // REMOVE THIS LATER - - if (m_isphysical && Body == IntPtr.Zero) { /* diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 233267e..56f3786 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -25,7 +25,6 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -//#define USE_DRAWSTUFF //#define SPAM using System; @@ -38,9 +37,6 @@ using System.Diagnostics; using log4net; using Nini.Config; using OdeAPI; -#if USE_DRAWSTUFF -using ODEDrawstuff; -#endif using OpenSim.Framework; using OpenSim.Region.Physics.Manager; using OpenMetaverse; @@ -366,31 +362,11 @@ namespace OpenSim.Region.Physics.OdePlugin //contactgroup d.WorldSetAutoDisableFlag(world, false); - #if USE_DRAWSTUFF - - Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); - viewthread.Start(); - #endif } _watermap = new float[258 * 258]; } -#if USE_DRAWSTUFF - public void startvisualization(object o) - { - ds.Functions fn; - fn.version = ds.VERSION; - fn.start = new ds.CallbackFunction(start); - fn.step = new ds.CallbackFunction(step); - fn.command = new ds.CallbackFunction(command); - fn.stop = null; - fn.path_to_textures = "./textures"; - string[] args = new string[0]; - ds.SimulationLoop(args.Length, args, 352, 288, ref fn); - } -#endif - // Initialize the mesh plugin // public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region ) public override void Initialise(IMesher meshmerizer, IConfigSource config) @@ -2560,134 +2536,5 @@ namespace OpenSim.Region.Physics.OdePlugin } return new List(); } - -#if USE_DRAWSTUFF - // Keyboard callback - public void command(int cmd) - { - IntPtr geom; - d.Mass mass; - d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f); - - - - Char ch = Char.ToLower((Char)cmd); - switch ((Char)ch) - { - case 'w': - try - { - Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); - - xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z; - ds.SetViewpoint(ref xyz, ref hpr); - } - catch (ArgumentException) - { hpr.X = 0; } - break; - - case 'a': - hpr.X++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - - case 's': - try - { - Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); - - xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z; - ds.SetViewpoint(ref xyz, ref hpr); - } - catch (ArgumentException) - { hpr.X = 0; } - break; - case 'd': - hpr.X--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'r': - xyz.Z++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'f': - xyz.Z--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'e': - xyz.Y++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'q': - xyz.Y--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - } - } - - public void step(int pause) - { - - ds.SetColor(1.0f, 1.0f, 0.0f); - ds.SetTexture(ds.Texture.Wood); - lock (_prims) - { - foreach (OdePrim prm in _prims) - { - //IntPtr body = d.GeomGetBody(prm.prim_geom); - if (prm.prim_geom != IntPtr.Zero) - { - d.Vector3 pos; - d.GeomCopyPosition(prm.prim_geom, out pos); - //d.BodyCopyPosition(body, out pos); - - d.Matrix3 R; - d.GeomCopyRotation(prm.prim_geom, out R); - //d.BodyCopyRotation(body, out R); - - - d.Vector3 sides = new d.Vector3(); - sides.X = prm.Size.X; - sides.Y = prm.Size.Y; - sides.Z = prm.Size.Z; - - ds.DrawBox(ref pos, ref R, ref sides); - } - } - } - ds.SetColor(1.0f, 0.0f, 0.0f); - lock (_characters) - { - foreach (OdeCharacter chr in _characters) - { - if (chr.Shell != IntPtr.Zero) - { - IntPtr body = d.GeomGetBody(chr.Shell); - - d.Vector3 pos; - d.GeomCopyPosition(chr.Shell, out pos); - //d.BodyCopyPosition(body, out pos); - - d.Matrix3 R; - d.GeomCopyRotation(chr.Shell, out R); - //d.BodyCopyRotation(body, out R); - - ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f); - d.Vector3 sides = new d.Vector3(); - sides.X = 0.5f; - sides.Y = 0.5f; - sides.Z = 0.5f; - - ds.DrawBox(ref pos, ref R, ref sides); - } - } - } - } - - public void start(int unused) - { - ds.SetViewpoint(ref xyz, ref hpr); - } -#endif } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs b/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs deleted file mode 100644 index aefad3a..0000000 --- a/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs +++ /dev/null @@ -1,99 +0,0 @@ -/* - * Copyright ODE - * Ode.NET - .NET bindings for ODE - * Jason Perkins (starkos@industriousone.com) - * Licensed under the New BSD - * Part of the OpenDynamicsEngine -Open Dynamics Engine -Copyright (c) 2001-2007, Russell L. Smith. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions -are met: - -Redistributions of source code must retain the above copyright notice, -this list of conditions and the following disclaimer. - -Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the following disclaimer in the documentation -and/or other materials provided with the distribution. - -Neither the names of ODE's copyright owner nor the names of its -contributors may be used to endorse or promote products derived from -this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * - */ - -using System; -using System.Runtime.InteropServices; -using OdeAPI; - -namespace ODEDrawstuff -{ -/*#if dDOUBLE - using dReal = System.Double; -#else - */ - using dReal = System.Single; -//#endif - - public static class ds - { - public const int VERSION = 2; - - public enum Texture - { - None, - Wood - } - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate void CallbackFunction(int arg); - - [StructLayout(LayoutKind.Sequential)] - public struct Functions - { - public int version; - public CallbackFunction start; - public CallbackFunction step; - public CallbackFunction command; - public CallbackFunction stop; - public string path_to_textures; - } - - [DllImport("drawstuff", EntryPoint = "dsDrawBox")] - public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides); - - [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")] - public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius); - - [DllImport("drawstuff", EntryPoint = "dsDrawConvex")] - public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); - - [DllImport("drawstuff", EntryPoint = "dsSetColor")] - public static extern void SetColor(float red, float green, float blue); - - [DllImport("drawstuff", EntryPoint = "dsSetTexture")] - public static extern void SetTexture(Texture texture); - - [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")] - public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr); - - [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")] - public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn); - } -} -- cgit v1.1 From f6f0d884bda8b5179d04a9cfe15efa3908552bcc Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 17:08:33 +0000 Subject: try to make crossings work better. chode no longer prevents crossings i hope --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 56 ++++++++++++++++++++++++--- 1 file changed, 50 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 42e22ff..95a9741 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -732,13 +732,13 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CrossingFailure() { - m_crossingfailures++; - if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + int tmp = Interlocked.Increment(ref m_crossingfailures); + if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) { base.RaiseOutOfBounds(_position); return; } - else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) { m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); } @@ -3042,10 +3042,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.Vector3 torque = d.BodyGetTorque(Body); _torque = new Vector3(torque.X, torque.Y, torque.Z); - base.RequestPhysicsterseUpdate(); //Console.WriteLine("Move {0} at {1}", m_primName, l_position); - + /* // Check if outside region // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) @@ -3112,7 +3111,52 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); return; // Dont process any other motion? } // end various methods } // end outside region horizontally - + */ + if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X + || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y + ) + { + // we are outside current region + // clip position to a stop just outside region and stop it only internally + // do it only once using m_crossingfailures as control + _position.X = Util.Clip(l_position.X, -0.2f, _parent_scene.WorldExtents.X + .2f); + _position.Y = Util.Clip(l_position.Y, -0.2f, _parent_scene.WorldExtents.Y + .2f); + _position.Z = Util.Clip(l_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + + if (Interlocked.Exchange(ref m_crossingfailures, 0) == 0) + { // tell base code only once + Interlocked.Increment(ref m_crossingfailures); + base.RequestPhysicsterseUpdate(); + } + return; + } + + if (Interlocked.Exchange(ref m_crossingfailures, 0) > 1) + { + // main simulator had a crossing failure + // park it inside region + _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(l_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + m_lastposition = _position; + + _velocity = Vector3.Zero; + m_lastVelocity = _velocity; + + + if (m_type != Vehicle.TYPE_NONE) + Halt(); + + d.BodySetLinearVel(Body, 0, 0, 0); + base.RequestPhysicsterseUpdate(); + return; + } + + base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) { -- cgit v1.1 From a758abaa9fa7250d1b61bcd906ca12539307851d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 17:08:33 +0000 Subject: try to make crossings work better. chode no longer prevents crossings i hope --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 56 ++++++++++++++++++++++++--- 1 file changed, 50 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 42e22ff..95a9741 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -732,13 +732,13 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CrossingFailure() { - m_crossingfailures++; - if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + int tmp = Interlocked.Increment(ref m_crossingfailures); + if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) { base.RaiseOutOfBounds(_position); return; } - else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) { m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); } @@ -3042,10 +3042,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.Vector3 torque = d.BodyGetTorque(Body); _torque = new Vector3(torque.X, torque.Y, torque.Z); - base.RequestPhysicsterseUpdate(); //Console.WriteLine("Move {0} at {1}", m_primName, l_position); - + /* // Check if outside region // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) @@ -3112,7 +3111,52 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); return; // Dont process any other motion? } // end various methods } // end outside region horizontally - + */ + if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X + || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y + ) + { + // we are outside current region + // clip position to a stop just outside region and stop it only internally + // do it only once using m_crossingfailures as control + _position.X = Util.Clip(l_position.X, -0.2f, _parent_scene.WorldExtents.X + .2f); + _position.Y = Util.Clip(l_position.Y, -0.2f, _parent_scene.WorldExtents.Y + .2f); + _position.Z = Util.Clip(l_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + + if (Interlocked.Exchange(ref m_crossingfailures, 0) == 0) + { // tell base code only once + Interlocked.Increment(ref m_crossingfailures); + base.RequestPhysicsterseUpdate(); + } + return; + } + + if (Interlocked.Exchange(ref m_crossingfailures, 0) > 1) + { + // main simulator had a crossing failure + // park it inside region + _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(l_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + m_lastposition = _position; + + _velocity = Vector3.Zero; + m_lastVelocity = _velocity; + + + if (m_type != Vehicle.TYPE_NONE) + Halt(); + + d.BodySetLinearVel(Body, 0, 0, 0); + base.RequestPhysicsterseUpdate(); + return; + } + + base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) { -- cgit v1.1 From b59275355e9dd8fbc126faf5bded8e61e84df4e8 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 19:44:54 +0000 Subject: don't freeze with a sitted avatar in a border without other sim. Still messy :( --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 95a9741..073241f 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -3125,7 +3125,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - if (Interlocked.Exchange(ref m_crossingfailures, 0) == 0) + if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) { // tell base code only once Interlocked.Increment(ref m_crossingfailures); base.RequestPhysicsterseUpdate(); @@ -3133,7 +3133,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); return; } - if (Interlocked.Exchange(ref m_crossingfailures, 0) > 1) + if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) { // main simulator had a crossing failure // park it inside region -- cgit v1.1 From f6c35cf26f759407268a46689d94e246a82b4286 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 20:33:31 +0000 Subject: Now if chode prim.cs detects out of bounds it requests a update and blocks movement and colisions. Base code must do a PhysActor.CrossingFailure() to make it move again inside sim or delete it. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 45 +++++++++++++++++++------ OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 20 +++++------ 2 files changed, 45 insertions(+), 20 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 073241f..a68dcb7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -166,7 +166,7 @@ namespace OpenSim.Region.Physics.OdePlugin public int m_roundsUnderMotionThreshold; private int m_crossingfailures; - public bool outofBounds; + public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; public bool _zeroFlag; // if body has been stopped @@ -732,6 +732,27 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CrossingFailure() { + if (m_outofBounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + m_lastposition = _position; + + _velocity = Vector3.Zero; + m_lastVelocity = _velocity; + + + if (m_type != Vehicle.TYPE_NONE) + Halt(); + + d.BodySetLinearVel(Body, 0, 0, 0); + base.RequestPhysicsterseUpdate(); + m_outofBounds = false; + } +/* int tmp = Interlocked.Increment(ref m_crossingfailures); if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) { @@ -742,6 +763,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); } + */ } public override float Buoyancy @@ -3011,7 +3033,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if(revcount > 0) revcount--; - if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims. + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. { // Old public void UpdatePositionAndVelocity(), more accuratley calculated here bool lastZeroFlag = _zeroFlag; // was it stopped @@ -3124,15 +3146,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); _position.Z = Util.Clip(l_position.Z, -100f, 50000f); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - - if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) - { // tell base code only once - Interlocked.Increment(ref m_crossingfailures); - base.RequestPhysicsterseUpdate(); - } + /* + if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) + { // tell base code only once + Interlocked.Increment(ref m_crossingfailures); + base.RequestPhysicsterseUpdate(); + } + */ + m_outofBounds = true; + base.RequestPhysicsterseUpdate(); return; } - +/* if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) { // main simulator had a crossing failure @@ -3155,7 +3180,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); base.RequestPhysicsterseUpdate(); return; } - +*/ base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 353db44..cf7c1d7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1536,7 +1536,7 @@ namespace OpenSim.Region.Physics.OdePlugin List removeprims = null; foreach (OdePrim chr in _activeprims) { - if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled)) + if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled) && !chr.m_outofBounds) { try { @@ -3410,13 +3410,13 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetTerrain(float[] heightMap, Vector3 pOffset) { - uint regionsize = (uint) Constants.RegionSize; // visible region size eg. 256(M) + int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M) - uint heightmapWidth = regionsize + 1; // ODE map size 257 x 257 (Meters) (1 extra - uint heightmapHeight = regionsize + 1; + int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra + int heightmapHeight = regionsize + 2; - uint heightmapWidthSamples = (uint)regionsize + 2; // Sample file size, 258 x 258 samples - uint heightmapHeightSamples = (uint)regionsize + 2; + int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples + int heightmapHeightSamples = (int)regionsize + 2; // Array of height samples for ODE float[] _heightmap; @@ -3432,10 +3432,10 @@ namespace OpenSim.Region.Physics.OdePlugin float hfmax = -2000f; float minele = 0.0f; // Dont allow -ve heights - uint x = 0; - uint y = 0; - uint xx = 0; - uint yy = 0; + int x = 0; + int y = 0; + int xx = 0; + int yy = 0; // load the height samples array from the heightMap for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257 -- cgit v1.1 From 3bffdddc9d0f0f62d7cf4bc6c271a865052ee119 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 19:44:54 +0000 Subject: don't freeze with a sitted avatar in a border without other sim. Still messy :( --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 95a9741..073241f 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -3125,7 +3125,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - if (Interlocked.Exchange(ref m_crossingfailures, 0) == 0) + if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) { // tell base code only once Interlocked.Increment(ref m_crossingfailures); base.RequestPhysicsterseUpdate(); @@ -3133,7 +3133,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); return; } - if (Interlocked.Exchange(ref m_crossingfailures, 0) > 1) + if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) { // main simulator had a crossing failure // park it inside region -- cgit v1.1 From 819fcdaee1745917072c0505416a71b5f40b60d0 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 20:33:31 +0000 Subject: Now if chode prim.cs detects out of bounds it requests a update and blocks movement and colisions. Base code must do a PhysActor.CrossingFailure() to make it move again inside sim or delete it. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 45 +++++++++++++++++++------ OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 20 +++++------ 2 files changed, 45 insertions(+), 20 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 073241f..a68dcb7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -166,7 +166,7 @@ namespace OpenSim.Region.Physics.OdePlugin public int m_roundsUnderMotionThreshold; private int m_crossingfailures; - public bool outofBounds; + public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; public bool _zeroFlag; // if body has been stopped @@ -732,6 +732,27 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CrossingFailure() { + if (m_outofBounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + m_lastposition = _position; + + _velocity = Vector3.Zero; + m_lastVelocity = _velocity; + + + if (m_type != Vehicle.TYPE_NONE) + Halt(); + + d.BodySetLinearVel(Body, 0, 0, 0); + base.RequestPhysicsterseUpdate(); + m_outofBounds = false; + } +/* int tmp = Interlocked.Increment(ref m_crossingfailures); if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) { @@ -742,6 +763,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); } + */ } public override float Buoyancy @@ -3011,7 +3033,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if(revcount > 0) revcount--; - if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims. + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. { // Old public void UpdatePositionAndVelocity(), more accuratley calculated here bool lastZeroFlag = _zeroFlag; // was it stopped @@ -3124,15 +3146,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); _position.Z = Util.Clip(l_position.Z, -100f, 50000f); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - - if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) - { // tell base code only once - Interlocked.Increment(ref m_crossingfailures); - base.RequestPhysicsterseUpdate(); - } + /* + if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) + { // tell base code only once + Interlocked.Increment(ref m_crossingfailures); + base.RequestPhysicsterseUpdate(); + } + */ + m_outofBounds = true; + base.RequestPhysicsterseUpdate(); return; } - +/* if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) { // main simulator had a crossing failure @@ -3155,7 +3180,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); base.RequestPhysicsterseUpdate(); return; } - +*/ base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 353db44..cf7c1d7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1536,7 +1536,7 @@ namespace OpenSim.Region.Physics.OdePlugin List removeprims = null; foreach (OdePrim chr in _activeprims) { - if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled)) + if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled) && !chr.m_outofBounds) { try { @@ -3410,13 +3410,13 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetTerrain(float[] heightMap, Vector3 pOffset) { - uint regionsize = (uint) Constants.RegionSize; // visible region size eg. 256(M) + int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M) - uint heightmapWidth = regionsize + 1; // ODE map size 257 x 257 (Meters) (1 extra - uint heightmapHeight = regionsize + 1; + int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra + int heightmapHeight = regionsize + 2; - uint heightmapWidthSamples = (uint)regionsize + 2; // Sample file size, 258 x 258 samples - uint heightmapHeightSamples = (uint)regionsize + 2; + int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples + int heightmapHeightSamples = (int)regionsize + 2; // Array of height samples for ODE float[] _heightmap; @@ -3432,10 +3432,10 @@ namespace OpenSim.Region.Physics.OdePlugin float hfmax = -2000f; float minele = 0.0f; // Dont allow -ve heights - uint x = 0; - uint y = 0; - uint xx = 0; - uint yy = 0; + int x = 0; + int y = 0; + int xx = 0; + int yy = 0; // load the height samples array from the heightMap for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257 -- cgit v1.1 From 7d77ccc6593c6c3ac9f66e2e593dfb8cc719cd04 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 17 Feb 2012 21:09:00 +0000 Subject: Added simple binary serializer/deserializer to chODE. 100% untested and most like still broken --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 2368 +++++++++++++---------- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 28 + OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs | 167 ++ OpenSim/Region/Physics/Manager/PhysicsActor.cs | 6 + OpenSim/Region/Physics/Manager/PhysicsScene.cs | 6 + 5 files changed, 1540 insertions(+), 1035 deletions(-) create mode 100644 OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index a68dcb7..6e28bfa 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -27,6 +27,7 @@ */ using System; +using System.IO; using System.Collections.Generic; using System.Reflection; using System.Runtime.InteropServices; @@ -48,6 +49,11 @@ namespace OpenSim.Region.Physics.OdePlugin { private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + public class SerialControl + { + public object alock = new object(); + public byte[] data = new byte[0]; + } private Vector3 _position; private Vector3 _velocity; private Vector3 _torque; @@ -80,12 +86,12 @@ namespace OpenSim.Region.Physics.OdePlugin private float PID_D = 35f; private float PID_G = 25f; private bool m_usePID = false; - + private Quaternion m_APIDTarget = new Quaternion(); private float m_APIDStrength = 0.5f; private float m_APIDDamping = 0.5f; private bool m_useAPID = false; - private float m_APIDdamper = 1.0f; + private float m_APIDdamper = 1.0f; // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), // do not confuse with VEHICLE HOVER @@ -112,7 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_taintPhysics; private bool m_collidesLand = true; private bool m_collidesWater; - public bool m_returnCollisions; + // public bool m_returnCollisions; // Default we're a Geometry private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); @@ -144,7 +150,7 @@ namespace OpenSim.Region.Physics.OdePlugin private OdeScene _parent_scene; public IntPtr m_targetSpace = IntPtr.Zero; public IntPtr prim_geom; -// public IntPtr prev_geom; + // public IntPtr prev_geom; public IntPtr _triMeshData; private IntPtr _linkJointGroup = IntPtr.Zero; @@ -163,8 +169,8 @@ namespace OpenSim.Region.Physics.OdePlugin private int throttleCounter; public int m_interpenetrationcount; public float m_collisionscore; - public int m_roundsUnderMotionThreshold; - private int m_crossingfailures; + // public int m_roundsUnderMotionThreshold; + // private int m_crossingfailures; public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; @@ -185,75 +191,470 @@ namespace OpenSim.Region.Physics.OdePlugin public volatile bool childPrim; internal int m_material = (int)Material.Wood; - - private int frcount = 0; // Used to limit dynamics debug output to - private int revcount = 0; // Reverse motion while > 0 private IntPtr m_body = IntPtr.Zero; - + // Vehicle properties ============================================================================================ private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier - private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings: - // HOVER_TERRAIN_ONLY - // HOVER_GLOBAL_HEIGHT - // NO_DEFLECTION_UP - // HOVER_WATER_ONLY - // HOVER_UP_ONLY - // LIMIT_MOTOR_UP - // LIMIT_ROLL_ONLY - + private VehicleFlag m_flags = (VehicleFlag)0; // Bit settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + // Linear properties private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity - //requested by LSL - private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL - private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL + //requested by LSL + private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL + private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL - - private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor - private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity - private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity - + + private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor + private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity + private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity + //Angular properties private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor -// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity + // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body - //Deflection properties + //Deflection properties // private float m_angularDeflectionEfficiency = 0; // private float m_angularDeflectionTimescale = 0; // private float m_linearDeflectionEfficiency = 0; // private float m_linearDeflectionTimescale = 0; - + //Banking properties // private float m_bankingEfficiency = 0; // private float m_bankingMix = 0; // private float m_bankingTimescale = 0; - + //Hover and Buoyancy properties private float m_VhoverHeight = 0f; -// private float m_VhoverEfficiency = 0f; + // private float m_VhoverEfficiency = 0f; private float m_VhoverTimescale = 0f; private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. - // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) - // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. - // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. - - //Attractor properties - private float m_verticalAttractionEfficiency = 1.0f; // damped + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. - - + SerialControl m_taintserial = null; + + public override byte[] Serialize(bool PhysIsRunning) + { + SerialControl sc = new SerialControl(); + + lock (sc.alock) + { + if (PhysIsRunning) + { + m_taintserial = sc; + + if (!Monitor.Wait(sc.alock, 1000)) + { + m_log.Error("[chOde] prim data serialization timed out"); + m_taintserial = null; + return new byte[0]; + } + } + else + DoSerialize(sc); + } + + return sc.data; + } + + public void DoSerialize(SerialControl sc) + { + wstreamer st = new wstreamer(); + Vector3 vtmp; + + ushort version = 2; + if (!BitConverter.IsLittleEndian) + version |= 1; + st.Wushort(version); //version lower bit codes endian type for future use + + // compact booleans in a ushort + ushort flags = 0; + + if (m_isphysical) // this should be true for now + flags |= 1; + if (m_isSelected) + flags |= 2; + if (m_isVolumeDetect) + flags |= 4; + if (m_disabled) + flags |= 8; + if (m_collidesWater) + flags |= 16; + if (m_collidesLand) + flags |= 32; + if (m_usePID) + flags |= 64; + if (m_useAPID) + flags |= 128; + if (m_useHoverPID) + flags |= 256; + if (m_throttleUpdates) + flags |= 512; + + st.Wushort(flags); + + st.Wvector3(_size); + st.Wint(m_material); + st.Wfloat(m_density); + st.Wfloat(0); // future gravity mod V3 + st.Wfloat(0); // future friction V3 + st.Wfloat(0); // future bounce V3 + +// st.Wuint((uint)m_collisionCategories); +// st.Wuint((uint)m_collisionFlags); + + if (_parent == null) + { + st.Wvector3(_position); // ?? + st.Wquat(_orientation); + } + else // for childs save offsets + { + Quaternion to; + Quaternion ipo = Quaternion.Inverse(_parent.Orientation); + + if (m_isphysical && prim_geom != IntPtr.Zero) + { + d.Vector3 dvt; + d.GeomCopyPosition(prim_geom, out dvt); + + vtmp.X = dvt.X; + vtmp.Y = dvt.Y; + vtmp.Z = dvt.Z; + + d.Quaternion dqt; + d.GeomCopyQuaternion(prim_geom, out dqt); + + to.X = dqt.X; + to.Y = dqt.Y; + to.Z = dqt.Z; + to.W = dqt.W; // rotation in world + } + else + { + vtmp = _position; + to = _orientation; + } + + vtmp -= _parent.Position; // offset in world + vtmp *= ipo; // offset in local + st.Wvector3(vtmp); + + ipo *= to; // own rotation + st.Wquat(ipo); + } + + st.Wvector3(_velocity); + st.Wvector3(m_rotationalVelocity); + st.Wvector3(_acceleration); + st.Wvector3(m_rotateEnable); + + vtmp = Vector3.Zero; + for (int i = 0; i < m_forcelist.Count; i++) + { + + vtmp += (m_forcelist[i] * 100); + } + + st.Wvector3(vtmp); // force acc + + vtmp = Vector3.Zero; + for (int i = 0; i < m_angularforcelist.Count; i++) + { + vtmp += (m_angularforcelist[i] * 100); + } + + st.Wvector3(vtmp); // angular force acc + + st.Wvector3(m_PIDTarget); + st.Wfloat(m_PIDTau); + st.Wfloat(PID_D); + st.Wfloat(PID_G); + st.Wquat(m_APIDTarget); + st.Wfloat(m_APIDStrength); + st.Wfloat(m_APIDDamping); + st.Wfloat(m_APIDdamper); + + st.Wint((int)m_PIDHoverType); + st.Wfloat(m_PIDHoverHeight); + st.Wfloat(m_PIDHoverTau); + st.Wfloat(m_targetHoverHeight); + + st.Wfloat(m_groundHeight); + st.Wfloat(m_waterHeight); + + st.Wfloat(m_buoyancy); + + // this must be last since type none ends stream + if (m_type == Vehicle.TYPE_NONE) + st.Wint((int)Vehicle.TYPE_NONE); + else + { + st.Wint((int)m_type); + + st.Wquat(Quaternion.Identity); //m_referenceFrame + + st.Wint((int)m_flags); + + st.Wvector3(m_linearMotorDirection); + st.Wfloat( + (float)Math.Sqrt(m_lLinMotorDVel.LengthSquared() / m_linearMotorDirection.LengthSquared())); + + st.Wvector3(m_linearFrictionTimescale); + st.Wfloat(m_linearMotorDecayTimescale); + st.Wfloat(m_linearMotorTimescale); + st.Wvector3(new Vector3(0, 0, 0)); //m_linearMotorOffset); + + st.Wvector3(m_angularMotorDirection); + st.Wfloat((float)Math.Sqrt(m_angularMotorDVel.LengthSquared() / m_angularMotorDirection.LengthSquared())); + + st.Wvector3(m_angularFrictionTimescale); + st.Wfloat(m_angularMotorDecayTimescale); + st.Wfloat(m_angularMotorTimescale); + + st.Wfloat(0); //m_linearDeflectionEfficiency); + st.Wfloat(1000); //m_linearDeflectionTimescale); + + st.Wfloat(0); //m_angularDeflectionEfficiency); + st.Wfloat(120); //m_angularDeflectionTimescale); + + st.Wfloat(0); // m_bankingEfficiency); + st.Wfloat(0); //m_bankingMix); + st.Wfloat(1000); //m_bankingTimescale); + st.Wfloat(m_VhoverHeight); + st.Wfloat(0.5f); //m_VhoverEfficiency); + st.Wfloat(m_VhoverTimescale); + st.Wfloat(m_VehicleBuoyancy); + + st.Wfloat(m_verticalAttractionEfficiency); + st.Wfloat(m_verticalAttractionTimescale); + } + sc.data = st.close(); + m_taintserial = null; + Monitor.PulseAll(sc.alock); + } + + public bool DeSerialize(byte[] data) + { + rstreamer st = new rstreamer(data); + + int version =st.Rushort(); //version + + // merge booleans in a ushort + ushort flags = st.Rushort(); + if ((flags & 1) != 0) + m_isphysical = true; + if ((flags & 2) != 0) + m_taintselected = true; + if ((flags & 4) != 0) + m_isVolumeDetect = true; + if ((flags & 8) != 0) + m_taintdisable = true; + if ((flags & 16) != 0) + m_taintCollidesWater = true; + if ((flags & 32) != 0) + m_collidesLand = true; + if ((flags & 64) != 0) + m_usePID = true; + if ((flags & 128) != 0) + m_useAPID = true; + if ((flags & 256) != 0) + m_useHoverPID = true; + if ((flags & 512) != 0) + m_throttleUpdates = true; + + _size = st.Rvector3(); + m_taintsize = _size; + + m_material= st.Rint(); + m_density = st.Rfloat(); + st.Rfloat(); // future gravity mod V3 + st.Rfloat(); // future friction V3 + st.Rfloat(); // future bounce V3 + +// m_collisionCategories = (CollisionCategories)st.Ruint(); +// m_collisionFlags = (CollisionCategories) st.Ruint(); + + if (m_taintparent == null) + { + st.Rvector3(); // ignore old position sop/sog as to tell the new one + m_taintrot = st.Rquat(); // + _orientation = m_taintrot; + } + else + { + m_taintrot = _parent.Orientation; + m_taintposition = st.Rvector3(); // ?? + _position = m_taintposition; + + m_taintposition *= m_taintrot; + m_taintposition += _parent.Position; + + m_taintrot *= st.Rquat(); // + _orientation = m_taintrot; + } + + m_taintVelocity = st.Rvector3(); + m_rotationalVelocity = st.Rvector3(); + + _acceleration = st.Rvector3(); + m_rotateEnableRequest = st.Rvector3(); + m_rotateEnableUpdate = true; + + Vector3 vtmp; + + vtmp = st.Rvector3(); // forces acc + m_forcelist.Add(vtmp); + m_taintforce = true; + + vtmp = st.Rvector3(); // angular forces acc + m_angularforcelist.Add(vtmp); + m_taintaddangularforce = true; + + m_PIDTarget = st.Rvector3(); + m_PIDTau = st.Rfloat(); + PID_D = st.Rfloat(); + PID_G = st.Rfloat(); + + m_APIDTarget = st.Rquat(); + m_APIDStrength = st.Rfloat(); + m_APIDDamping = st.Rfloat(); + m_APIDdamper = st.Rfloat(); + + m_PIDHoverType = (PIDHoverType) st.Rint(); + m_PIDHoverHeight = st.Rfloat(); + m_PIDHoverTau = st.Rfloat(); + m_targetHoverHeight = st.Rfloat(); + + m_groundHeight = st.Rfloat(); + m_waterHeight = st.Rfloat(); + + m_buoyancy = st.Rfloat(); + + + // this must be last since type none ends stream + + m_type = (Vehicle) st.Rint(); + + if (m_type != Vehicle.TYPE_NONE) + { + float ftmp; + + st.Rquat(); //m_referenceFrame + + m_flags = (VehicleFlag) st.Rint(); + + m_linearMotorDirection = st.Rvector3(); + + ftmp = st.Rfloat(); + m_lLinMotorDVel = m_linearMotorDirection * ftmp; + + m_linearFrictionTimescale = st.Rvector3(); + m_linearMotorDecayTimescale = st.Rfloat(); + m_linearMotorTimescale = st.Rfloat(); + st.Rvector3(); //m_linearMotorOffset); + + m_angularMotorDirection = st.Rvector3(); + ftmp = st.Rfloat(); + m_angularMotorDVel = m_angularMotorDirection * ftmp; + + m_angularFrictionTimescale = st.Rvector3(); + m_angularMotorDecayTimescale = st.Rfloat(); + m_angularMotorTimescale = st.Rfloat(); + + st.Rfloat(); //m_linearDeflectionEfficiency); + st.Rfloat(); //m_linearDeflectionTimescale); + + st.Rfloat(); //m_angularDeflectionEfficiency); + st.Rfloat(); //m_angularDeflectionTimescale); + + st.Rfloat(); // m_bankingEfficiency); + st.Rfloat(); //m_bankingMix); + st.Rfloat(); //m_bankingTimescale); + + m_VhoverHeight = st.Rfloat(); + st.Rfloat(); //m_VhoverEfficiency); + m_VhoverTimescale = st.Rfloat(); + + m_VehicleBuoyancy = st.Rfloat(); + + m_verticalAttractionEfficiency = st.Rfloat(); + m_verticalAttractionTimescale = st.Rfloat(); + } + st.close(); + return true; + } + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, PhysicsActor parent, + PrimitiveBaseShape pbs, CollisionLocker dode, uint localid, byte[] sdata) + { + m_localID = localid; + ode = dode; + + if (parent == null) + { + m_taintparent = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); + } + + _position = pos; + m_taintposition = pos; + } + else + m_taintparent = parent; + + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + + _mesh = null; + m_meshfailed = false; + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = (IntPtr)0; + + if(sdata != null && sdata.Length > 1) + DeSerialize(sdata); + + if (m_isphysical) + m_targetSpace = _parent_scene.space; + + m_primName = primName; + m_taintserial = null; + m_taintadd = true; + _parent_scene.AddPhysicsActorTaint(this); + // don't do .add() here; old geoms get recycled with the same hash + } public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) @@ -266,7 +667,7 @@ namespace OpenSim.Region.Physics.OdePlugin parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); } - + _position = pos; m_taintposition = pos; PID_D = parent_scene.bodyPIDD; @@ -275,9 +676,8 @@ namespace OpenSim.Region.Physics.OdePlugin // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - prim_geom = IntPtr.Zero; -// prev_geom = IntPtr.Zero; + // prev_geom = IntPtr.Zero; if (!pos.IsFinite()) { @@ -306,7 +706,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene = parent_scene; m_targetSpace = (IntPtr)0; -// if (pos.Z < 0) + // if (pos.Z < 0) if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) m_isphysical = false; else @@ -317,6 +717,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical) m_targetSpace = _parent_scene.space; } + + m_taintserial = null; m_primName = primName; m_taintadd = true; _parent_scene.AddPhysicsActorTaint(this); @@ -325,7 +727,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override int PhysicsActorType { - get { return (int) ActorTypes.Prim; } + get { return (int)ActorTypes.Prim; } set { return; } } @@ -337,9 +739,11 @@ namespace OpenSim.Region.Physics.OdePlugin public override uint LocalID { - set { + set + { //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); - m_localID = value; } + m_localID = value; + } } public override bool Grabbed @@ -349,9 +753,10 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool Selected { - set { - -//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); + set + { + + //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); // This only makes the object not collidable if the object // is physical or the object is modified somehow *IN THE FUTURE* // without this, if an avatar selects prim, they can walk right @@ -367,21 +772,21 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintselected = value; m_isSelected = value; } - if(m_isSelected) disableBodySoft(); + if (m_isSelected) disableBodySoft(); } } public override bool IsPhysical { get { return m_isphysical; } - set - { - m_isphysical = value; - if (!m_isphysical) - { // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - if (m_type != Vehicle.TYPE_NONE) Halt(); - } + set + { + m_isphysical = value; + if (!m_isphysical) + { // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + if (m_type != Vehicle.TYPE_NONE) Halt(); + } } } @@ -430,7 +835,9 @@ namespace OpenSim.Region.Physics.OdePlugin { get { return _position; } - set { _position = value; + set + { + _position = value; //m_log.Info("[PHYSICS]: " + _position.ToString()); } } @@ -481,29 +888,29 @@ namespace OpenSim.Region.Physics.OdePlugin public override void VehicleFloatParam(int param, float value) { - ProcessFloatVehicleParam((Vehicle) param, value); + ProcessFloatVehicleParam((Vehicle)param, value); } public override void VehicleVectorParam(int param, Vector3 value) { - ProcessVectorVehicleParam((Vehicle) param, value); + ProcessVectorVehicleParam((Vehicle)param, value); } public override void VehicleRotationParam(int param, Quaternion rotation) { - ProcessRotationVehicleParam((Vehicle) param, rotation); + ProcessRotationVehicleParam((Vehicle)param, rotation); } public override void VehicleFlags(int param, bool remove) { ProcessVehicleFlags(param, remove); } - + public override void SetVolumeDetect(int param) { lock (_parent_scene.OdeLock) { - m_isVolumeDetect = (param!=0); + m_isVolumeDetect = (param != 0); } } @@ -536,9 +943,9 @@ namespace OpenSim.Region.Physics.OdePlugin return Vector3.Zero; Vector3 returnVelocity = Vector3.Zero; - returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; - returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; - returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; + returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; return returnVelocity; } set @@ -546,8 +953,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { _velocity = value; - if (_velocity.ApproxEquals(Vector3.Zero,0.001f)) - _acceleration = Vector3.Zero; + if (_velocity.ApproxEquals(Vector3.Zero, 0.001f)) + _acceleration = Vector3.Zero; m_taintVelocity = value; _parent_scene.AddPhysicsActorTaint(this); @@ -604,17 +1011,18 @@ namespace OpenSim.Region.Physics.OdePlugin if (QuaternionIsFinite(value)) { _orientation = value; - } + } else m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); } } - - - public override bool FloatOnWater + + + public override bool FloatOnWater { - set { + set + { m_taintCollidesWater = value; _parent_scene.AddPhysicsActorTaint(this); } @@ -624,8 +1032,8 @@ namespace OpenSim.Region.Physics.OdePlugin { } - public override Vector3 PIDTarget - { + public override Vector3 PIDTarget + { set { if (value.IsFinite()) @@ -634,16 +1042,16 @@ namespace OpenSim.Region.Physics.OdePlugin } else m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); - } + } } public override bool PIDActive { set { m_usePID = value; } } public override float PIDTau { set { m_PIDTau = value; } } - - // For RotLookAt - public override Quaternion APIDTarget { set { m_APIDTarget = value; } } - public override bool APIDActive { set { m_useAPID = value; } } - public override float APIDStrength { set { m_APIDStrength = value; } } - public override float APIDDamping { set { m_APIDDamping = value; } } + + // For RotLookAt + public override Quaternion APIDTarget { set { m_APIDTarget = value; } } + public override bool APIDActive { set { m_useAPID = value; } } + public override float APIDStrength { set { m_APIDStrength = value; } } + public override float APIDDamping { set { m_APIDDamping = value; } } public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } public override bool PIDHoverActive { set { m_useHoverPID = value; } } @@ -665,13 +1073,13 @@ namespace OpenSim.Region.Physics.OdePlugin public override Vector3 Acceleration // client updates read data via here { - get - { + get + { if (_zeroFlag) { return Vector3.Zero; } - return _acceleration; + return _acceleration; } set { _acceleration = value; } } @@ -752,18 +1160,18 @@ namespace OpenSim.Region.Physics.OdePlugin base.RequestPhysicsterseUpdate(); m_outofBounds = false; } -/* - int tmp = Interlocked.Increment(ref m_crossingfailures); - if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - base.RaiseOutOfBounds(_position); - return; - } - else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); - } - */ + /* + int tmp = Interlocked.Increment(ref m_crossingfailures); + if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + base.RaiseOutOfBounds(_position); + return; + } + else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); + } + */ } public override float Buoyancy @@ -806,26 +1214,26 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetGeom(IntPtr geom) { - if(prim_geom != IntPtr.Zero) - { - // Remove any old entries -//string tPA; -//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); -//Console.WriteLine("**** Remove {0}", tPA); - if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); - if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); + if (prim_geom != IntPtr.Zero) + { + // Remove any old entries + //string tPA; + //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); + //Console.WriteLine("**** Remove {0}", tPA); + if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); + if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); d.GeomDestroy(prim_geom); - } + } prim_geom = geom; -//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); + //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); if (prim_geom != IntPtr.Zero) { _parent_scene.geom_name_map[prim_geom] = this.m_primName; _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); -//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); + //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); } if (childPrim) @@ -833,7 +1241,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null && _parent is OdePrim) { OdePrim parent = (OdePrim)_parent; -//Console.WriteLine("SetGeom calls ChildSetGeom"); + //Console.WriteLine("SetGeom calls ChildSetGeom"); parent.ChildSetGeom(this); } } @@ -848,7 +1256,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodyEnable(Body); if (m_type != Vehicle.TYPE_NONE) - Enable(Body, _parent_scene); + Enable(Body, _parent_scene); } m_disabled = false; @@ -892,9 +1300,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - + // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode (Body, false); + d.BodySetGravityMode(Body, false); m_interpenetrationcount = 0; m_collisionscore = 0; @@ -918,19 +1326,19 @@ namespace OpenSim.Region.Physics.OdePlugin float returnMass = 0; float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - + float hollowVolume = hollowAmount * hollowAmount; + switch (_pbs.ProfileShape) { case ProfileShape.Square: // default box if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { if (hollowAmount > 0.0) - { + { switch (_pbs.HollowShape) - { + { case HollowShape.Square: case HollowShape.Same: break; @@ -948,31 +1356,31 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { //a tube volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume*tmp*tmp; - + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + if (hollowAmount > 0.0) - { + { hollowVolume *= hollowAmount; - + switch (_pbs.HollowShape) - { + { case HollowShape.Square: case HollowShape.Same: break; case HollowShape.Circle: - hollowVolume *= 0.78539816339f;; + hollowVolume *= 0.78539816339f; ; break; case HollowShape.Triangle: @@ -981,23 +1389,23 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } break; case ProfileShape.Circle: if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { volume *= 0.78539816339f; // elipse base if (hollowAmount > 0.0) - { + { switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Circle: break; @@ -1013,25 +1421,25 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); volume *= (1.0f - tmp * tmp); - + if (hollowAmount > 0.0) - { + { // calculate the hollow volume by it's shape compared to the prim shape hollowVolume *= hollowAmount; switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Circle: break; @@ -1047,31 +1455,31 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } break; case ProfileShape.HalfCircle: if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - volume *= 0.52359877559829887307710723054658f; + volume *= 0.52359877559829887307710723054658f; } break; case ProfileShape.EquilateralTriangle: if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { volume *= 0.32475953f; if (hollowAmount > 0.0) - { + { // calculate the hollow volume by it's shape compared to the prim shape switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Triangle: hollowVolume *= .25f; @@ -1091,24 +1499,24 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { volume *= 0.32475953f; volume *= 0.01f * (float)(200 - _pbs.PathScaleX); tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); volume *= (1.0f - tmp * tmp); if (hollowAmount > 0.0) - { + { hollowVolume *= hollowAmount; switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Triangle: hollowVolume *= .25f; @@ -1126,15 +1534,15 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } - break; + } + break; default: break; - } + } @@ -1148,7 +1556,7 @@ namespace OpenSim.Region.Physics.OdePlugin float profileEnd; if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) - { + { taperX1 = _pbs.PathScaleX * 0.01f; if (taperX1 > 1.0f) taperX1 = 2.0f - taperX1; @@ -1158,9 +1566,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (taperY1 > 1.0f) taperY1 = 2.0f - taperY1; taperY = 1.0f - taperY1; - } + } else - { + { taperX = _pbs.PathTaperX * 0.01f; if (taperX < 0.0f) taperX = -taperX; @@ -1168,10 +1576,10 @@ namespace OpenSim.Region.Physics.OdePlugin taperY = _pbs.PathTaperY * 0.01f; if (taperY < 0.0f) - taperY = -taperY; + taperY = -taperY; taperY1 = 1.0f - taperY; - } + } volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); @@ -1180,7 +1588,7 @@ namespace OpenSim.Region.Physics.OdePlugin pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; volume *= (pathEnd - pathBegin); -// this is crude aproximation + // this is crude aproximation profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; volume *= (profileEnd - profileBegin); @@ -1189,8 +1597,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. -// else if (returnMass > _parent_scene.maximumMassObject) -// returnMass = _parent_scene.maximumMassObject; + // else if (returnMass > _parent_scene.maximumMassObject) + // returnMass = _parent_scene.maximumMassObject; @@ -1230,7 +1638,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void setMass() { - if (Body != (IntPtr) 0) + if (Body != (IntPtr)0) { float newmass = CalculateMass(); @@ -1260,7 +1668,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - + d.BodyDestroy(Body); lock (childrenPrim) { @@ -1279,7 +1687,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { _parent_scene.remActivePrim(this); - + m_collisionCategories &= ~CollisionCategories.Body; m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); @@ -1289,7 +1697,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - + Body = IntPtr.Zero; } } @@ -1345,10 +1753,10 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer - // { - SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); - // } + // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer + // { + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); + // } } catch (AccessViolationException) { @@ -1357,14 +1765,14 @@ namespace OpenSim.Region.Physics.OdePlugin } - // if (IsPhysical && Body == (IntPtr) 0) - // { - // Recreate the body - // m_interpenetrationcount = 0; - // m_collisionscore = 0; + // if (IsPhysical && Body == (IntPtr) 0) + // { + // Recreate the body + // m_interpenetrationcount = 0; + // m_collisionscore = 0; - // enableBody(); - // } + // enableBody(); + // } } public void ProcessTaints(float timestep) //============================================================================= @@ -1373,37 +1781,37 @@ namespace OpenSim.Region.Physics.OdePlugin { changeadd(timestep); } - + if (prim_geom != IntPtr.Zero) { - if (!_position.ApproxEquals(m_taintposition, 0f)) + if (!_position.ApproxEquals(m_taintposition, 0f)) { - changemove(timestep); + changemove(timestep); } - if (m_taintrot != _orientation) - { - if(childPrim && IsPhysical) // For physical child prim... - { - rotate(timestep); - // KF: ODE will also rotate the parent prim! - // so rotate the root back to where it was - OdePrim parent = (OdePrim)_parent; - parent.rotate(timestep); + if (m_taintrot != _orientation) + { + if (childPrim && IsPhysical) // For physical child prim... + { + rotate(timestep); + // KF: ODE will also rotate the parent prim! + // so rotate the root back to where it was + OdePrim parent = (OdePrim)_parent; + parent.rotate(timestep); } else { - //Just rotate the prim - rotate(timestep); - } + //Just rotate the prim + rotate(timestep); + } } // - + if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) { changePhysicsStatus(timestep); }// - if (!_size.ApproxEquals(m_taintsize,0f)) + if (!_size.ApproxEquals(m_taintsize, 0f)) changesize(timestep); // @@ -1434,10 +1842,14 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); -/* obsolete - if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) - changeAngularLock(timestep); - */ + + if (m_taintserial != null) + DoSerialize(m_taintserial); + + /* obsolete + if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) + changeAngularLock(timestep); + */ } else { @@ -1445,16 +1857,16 @@ namespace OpenSim.Region.Physics.OdePlugin } } -/* obsolete - private void changeAngularLock(float timestep) - { - if (_parent == null) - { - m_angularLock = m_taintAngularLock; - m_angularLockSet = true; - } - } - */ + /* obsolete + private void changeAngularLock(float timestep) + { + if (_parent == null) + { + m_angularLock = m_taintAngularLock; + m_angularLockSet = true; + } + } + */ private void changelink(float timestep) { // If the newly set parent is not null @@ -1489,7 +1901,7 @@ namespace OpenSim.Region.Physics.OdePlugin childPrim = false; //_parent = null; } - + /* if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) d.JointGroupDestroy(_linkJointGroup); @@ -1498,7 +1910,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linkJoint = (IntPtr)0; */ } - + _parent = m_taintparent; m_taintPhysics = m_isphysical; } @@ -1512,8 +1924,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body == IntPtr.Zero) { Body = d.BodyCreate(_parent_scene.world); - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode (Body, false); + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); setMass(); } @@ -1524,7 +1936,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childrenPrim.Contains(prim)) { childrenPrim.Add(prim); - + foreach (OdePrim prm in childrenPrim) { d.Mass m2; @@ -1546,7 +1958,7 @@ namespace OpenSim.Region.Physics.OdePlugin } foreach (OdePrim prm in childrenPrim) { - + prm.m_collisionCategories |= CollisionCategories.Body; prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); @@ -1571,7 +1983,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.GeomSetBody(prm.prim_geom, Body); prm.childPrim = true; - d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); + d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z); //d.GeomSetOffsetPosition(prim.prim_geom, // (Position.X - prm.Position.X) - pMass.c.X, // (Position.Y - prm.Position.Y) - pMass.c.Y, @@ -1668,7 +2080,7 @@ namespace OpenSim.Region.Physics.OdePlugin ParentPrim(prm); } } - + } private void ChildDelink(OdePrim odePrim) @@ -1719,7 +2131,7 @@ namespace OpenSim.Region.Physics.OdePlugin // in between the disabling and the collision properties setting // which would wake the physical body up from a soft disabling and potentially cause it to fall // through the ground. - + // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select // just one part of the assembly, the rest of the assembly is non-selected and still simulating, // so that causes the selected part to wake up and continue moving. @@ -1752,12 +2164,12 @@ namespace OpenSim.Region.Physics.OdePlugin { disableBodySoft(); } - if (Body != IntPtr.Zero) + if (Body != IntPtr.Zero) { d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); } } @@ -1780,12 +2192,12 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - if (Body != IntPtr.Zero) + if (Body != IntPtr.Zero) { d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); } if (m_isphysical) @@ -1913,7 +2325,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_meshfailed = true; } // createmesh returns null when it's a shape that isn't a cube. - // m_log.Debug(m_localID); + // m_log.Debug(m_localID); } } @@ -1948,7 +2360,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical) { -// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! + // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! if (!m_taintremove && !childPrim) { if (Body == IntPtr.Zero) @@ -1972,8 +2384,8 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim odParent = (OdePrim)_parent; if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) { -// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? -Console.WriteLine("ODEPrim JointCreateFixed !!!"); + // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? + Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); d.JointAttach(m_linkJoint, Body, odParent.Body); d.JointSetFixed(m_linkJoint); @@ -1991,8 +2403,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } } //else - // { - //m_log.Debug("[BUG]: race!"); + // { + //m_log.Debug("[BUG]: race!"); //} } else @@ -2031,15 +2443,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); myrot.W = _orientation.W; if (Body != IntPtr.Zero) { - // KF: If this is a root prim do BodySet + // KF: If this is a root prim do BodySet d.BodySetQuaternion(Body, ref myrot); - } - else - { - // daughter prim, do Geom set - d.GeomSetQuaternion(prim_geom, ref myrot); } - + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); + } + resetCollisionAccounting(); m_taintrot = _orientation; } @@ -2111,7 +2523,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public void changesize(float timestamp) { - + string oldname = _parent_scene.geom_name_map[prim_geom]; if (_size.X <= 0) _size.X = 0.01f; @@ -2170,7 +2582,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); CreateGeom(m_targetSpace, mesh); - + } else { @@ -2210,7 +2622,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_taintsize = _size; } - + public void changefloatonwater(float timestep) { @@ -2398,7 +2810,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } d.BodyEnable(Body); d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); - + } m_angularforcelist.Clear(); } @@ -2420,7 +2832,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if (Body != IntPtr.Zero) d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); } - + //resetCollisionAccounting(); } m_taintVelocity = Vector3.Zero; @@ -2428,9 +2840,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public void UpdatePositionAndVelocity() { - return; // moved to the Move () method + return; // moved to the Move () method } - + public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) { obj.I.M00 = pMat[0, 0]; @@ -2455,7 +2867,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { _parent_scene.remCollisionEventReporting(this); m_eventsubscription = 0; - } + } public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { @@ -2499,9 +2911,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public static Matrix4 Adjoint(Matrix4 pMat) { Matrix4 adjointMatrix = new Matrix4(); - for (int i=0; i<4; i++) + for (int i = 0; i < 4; i++) { - for (int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); } @@ -2524,7 +2936,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { if (j == iCol) continue; - Matrix4SetValue(ref minor, m,n, matrix[i, j]); + Matrix4SetValue(ref minor, m, n, matrix[i, j]); n++; } m++; @@ -2622,18 +3034,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); private static float determinant3x3(Matrix4 pMat) { float det = 0; - float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; - float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; - float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; - float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; - float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; - float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; + float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2]; + float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0]; + float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1]; + float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2]; + float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0]; + float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1]; det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); return det; } - + private static void DMassCopy(ref d.Mass src, ref d.Mass dst) { dst.c.W = src.c.W; @@ -2690,15 +3102,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_bankingTimescale = pValue; break; case Vehicle.BUOYANCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; m_VehicleBuoyancy = pValue; break; -// case Vehicle.HOVER_EFFICIENCY: -// if (pValue < 0f) pValue = 0f; -// if (pValue > 1f) pValue = 1f; -// m_VhoverEfficiency = pValue; -// break; + // case Vehicle.HOVER_EFFICIENCY: + // if (pValue < 0f) pValue = 0f; + // if (pValue > 1f) pValue = 1f; + // m_VhoverEfficiency = pValue; + // break; case Vehicle.HOVER_HEIGHT: m_VhoverHeight = pValue; break; @@ -2731,12 +3143,12 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if (pValue < 0.1f) pValue = 0.1f; m_verticalAttractionTimescale = pValue; break; - + // These are vector properties but the engine lets you use a single float value to // set all of the components to the same value case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue > 30f) pValue = 30f; - if (pValue < 0.1f) pValue = 0.1f; + if (pValue > 30f) pValue = 30f; + if (pValue < 0.1f) pValue = 0.1f; m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: @@ -2744,7 +3156,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue < 0.1f) pValue = 0.1f; + if (pValue < 0.1f) pValue = 0.1f; m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -2756,7 +3168,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); break; } - + }//end ProcessFloatVehicleParam internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) @@ -2764,29 +3176,29 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); switch (pParam) { case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue.X > 30f) pValue.X = 30f; - if (pValue.X < 0.1f) pValue.X = 0.1f; - if (pValue.Y > 30f) pValue.Y = 30f; - if (pValue.Y < 0.1f) pValue.Y = 0.1f; - if (pValue.Z > 30f) pValue.Z = 30f; - if (pValue.Z < 0.1f) pValue.Z = 0.1f; + if (pValue.X > 30f) pValue.X = 30f; + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y > 30f) pValue.Y = 30f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z > 30f) pValue.Z = 30f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // Limit requested angular speed to 2 rps= 4 pi rads/sec - if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; - if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; - if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; - if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; - if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; - if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; + if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; + if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f; + if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; + if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f; + if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; + if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f; UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue.X < 0.1f) pValue.X = 0.1f; - if (pValue.Y < 0.1f) pValue.Y = 0.1f; - if (pValue.Z < 0.1f) pValue.Z = 0.1f; + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -2797,7 +3209,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); break; } - + }//end ProcessVectorVehicleParam internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) @@ -2808,31 +3220,31 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_referenceFrame = pValue; break; } - + }//end ProcessRotationVehicleParam - + internal void ProcessVehicleFlags(int pParam, bool remove) { if (remove) { - m_flags &= ~((VehicleFlag)pParam); - } - else - { - m_flags |= (VehicleFlag)pParam; - } + m_flags &= ~((VehicleFlag)pParam); + } + else + { + m_flags |= (VehicleFlag)pParam; + } } - + internal void ProcessTypeChange(Vehicle pType) { - // Set Defaults For Type + // Set Defaults For Type m_type = pType; switch (pType) { case Vehicle.TYPE_SLED: m_linearFrictionTimescale = new Vector3(30, 1, 1000); m_angularFrictionTimescale = new Vector3(30, 30, 30); -// m_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1000; m_linearMotorDecayTimescale = 120; m_angularMotorDirection = Vector3.Zero; @@ -2840,7 +3252,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 1000; m_angularMotorDecayTimescale = 120; m_VhoverHeight = 0; -// m_VhoverEfficiency = 1; + // m_VhoverEfficiency = 1; m_VhoverTimescale = 10; m_VehicleBuoyancy = 0; // m_linearDeflectionEfficiency = 1; @@ -2859,7 +3271,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); case Vehicle.TYPE_CAR: m_linearFrictionTimescale = new Vector3(100, 2, 1000); m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. -// m_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2867,7 +3279,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 1; m_angularMotorDecayTimescale = 0.8f; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0; + // m_VhoverEfficiency = 0; m_VhoverTimescale = 1000; m_VehicleBuoyancy = 0; // // m_linearDeflectionEfficiency = 1; @@ -2886,8 +3298,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); break; case Vehicle.TYPE_BOAT: m_linearFrictionTimescale = new Vector3(10, 3, 2); - m_angularFrictionTimescale = new Vector3(10,10,10); -// m_lLinMotorVel = Vector3.Zero; + m_angularFrictionTimescale = new Vector3(10, 10, 10); + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 5; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2895,7 +3307,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0.5f; + // m_VhoverEfficiency = 0.5f; m_VhoverTimescale = 2; m_VehicleBuoyancy = 1; // m_linearDeflectionEfficiency = 0.5f; @@ -2908,15 +3320,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_bankingMix = 0.8f; // m_bankingTimescale = 1; // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.LIMIT_MOTOR_UP); break; case Vehicle.TYPE_AIRPLANE: m_linearFrictionTimescale = new Vector3(200, 10, 5); m_angularFrictionTimescale = new Vector3(20, 20, 20); -// m_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 2; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2924,7 +3336,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0.5f; + // m_VhoverEfficiency = 0.5f; m_VhoverTimescale = 1000; m_VehicleBuoyancy = 0; // m_linearDeflectionEfficiency = 0.5f; @@ -2951,7 +3363,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 6; m_angularMotorDecayTimescale = 10; m_VhoverHeight = 5; -// m_VhoverEfficiency = 0.8f; + // m_VhoverEfficiency = 0.8f; m_VhoverTimescale = 10; m_VehicleBuoyancy = 1; // m_linearDeflectionEfficiency = 0; @@ -2981,63 +3393,48 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } - internal void Halt() - { // Kill all motions, when non-physical - // m_linearMotorDirection = Vector3.Zero; - m_lLinMotorDVel = Vector3.Zero; - m_lLinObjectVel = Vector3.Zero; - m_wLinObjectVel = Vector3.Zero; - m_angularMotorDirection = Vector3.Zero; - m_lastAngularVelocity = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - _acceleration = Vector3.Zero; - } - - private void UpdateLinDecay() - { -// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; -// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; -// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; - m_lLinMotorDVel.X = m_linearMotorDirection.X; - m_lLinMotorDVel.Y = m_linearMotorDirection.Y; - m_lLinMotorDVel.Z = m_linearMotorDirection.Z; - } // else let the motor decay on its own - - private void UpdateAngDecay() - { -// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; -// if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; -// if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; - m_angularMotorDVel.X = m_angularMotorDirection.X; - m_angularMotorDVel.Y = m_angularMotorDirection.Y; - m_angularMotorDVel.Z = m_angularMotorDirection.Z; - } // else let the motor decay on its own - + internal void Halt() + { // Kill all motions, when non-physical + // m_linearMotorDirection = Vector3.Zero; + m_lLinMotorDVel = Vector3.Zero; + m_lLinObjectVel = Vector3.Zero; + m_wLinObjectVel = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + m_lastAngularVelocity = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + _acceleration = Vector3.Zero; + } + + private void UpdateLinDecay() + { + m_lLinMotorDVel.X = m_linearMotorDirection.X; + m_lLinMotorDVel.Y = m_linearMotorDirection.Y; + m_lLinMotorDVel.Z = m_linearMotorDirection.Z; + } // else let the motor decay on its own + + private void UpdateAngDecay() + { + m_angularMotorDVel.X = m_angularMotorDirection.X; + m_angularMotorDVel.Y = m_angularMotorDirection.Y; + m_angularMotorDVel.Z = m_angularMotorDirection.Z; + } // else let the motor decay on its own + public void Move(float timestep) { float fx = 0; float fy = 0; float fz = 0; Vector3 linvel; // velocity applied, including any reversal - int outside = 0; - + // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. // This is a temp patch until proper region crossing is developed. - - int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; - float fence = _parent_scene.geomRegionFence; - - frcount++; // used to limit debug comment output - if (frcount > 50) - frcount = 0; - - if(revcount > 0) revcount--; - + + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. { - // Old public void UpdatePositionAndVelocity(), more accuratley calculated here - bool lastZeroFlag = _zeroFlag; // was it stopped - + // Old public void UpdatePositionAndVelocity(), more accuratley calculated here + bool lastZeroFlag = _zeroFlag; // was it stopped + d.Vector3 vec = d.BodyGetPosition(Body); Vector3 l_position = Vector3.Zero; l_position.X = vec.X; @@ -3045,95 +3442,26 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); l_position.Z = vec.Z; m_lastposition = _position; _position = l_position; - + d.Quaternion ori = d.BodyGetQuaternion(Body); - // Quaternion l_orientation = Quaternion.Identity; + // Quaternion l_orientation = Quaternion.Identity; _orientation.X = ori.X; _orientation.Y = ori.Y; _orientation.Z = ori.Z; _orientation.W = ori.W; m_lastorientation = _orientation; - + d.Vector3 vel = d.BodyGetLinearVel(Body); m_lastVelocity = _velocity; _velocity.X = vel.X; _velocity.Y = vel.Y; _velocity.Z = vel.Z; _acceleration = ((_velocity - m_lastVelocity) / timestep); - + d.Vector3 torque = d.BodyGetTorque(Body); _torque = new Vector3(torque.X, torque.Y, torque.Z); - - -//Console.WriteLine("Move {0} at {1}", m_primName, l_position); - /* - // Check if outside region - // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! - if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) - { - l_position.X = ((float)_parent_scene.WorldExtents.X - fence); - outside = 1; - } - - if (l_position.X < fence) - { - l_position.X = fence; - outside = 2; - } - if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence)) - { - l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence); - outside = 3; - } - - if (l_position.Y < fence) - { - l_position.Y = fence; - outside = 4; - } - - if (outside > 0) - { - -//Console.WriteLine("Border {0} fence={1}", l_position, fence); - if (fence > 0.0f) // bounce object off boundary - { - if (revcount == 0) - { - if (outside < 3) - { - _velocity.X = -_velocity.X; - } - else - { - _velocity.Y = -_velocity.Y; - } - if (m_type != Vehicle.TYPE_NONE) Halt(); - _position = l_position; - m_taintposition = _position; - m_lastVelocity = _velocity; - _acceleration = Vector3.Zero; - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - base.RequestPhysicsterseUpdate(); - - revcount = 25; // wait for object to move away from border - } - } // else old crossing mode - else if (m_crossingfailures < failureLimit) - { // keep trying to cross? - _position = l_position; - //_parent_scene.remActivePrim(this); - if (_parent == null) base.RequestPhysicsterseUpdate(); - return; // Dont process any other motion? - } - else - { // Too many tries - if (_parent == null) base.RaiseOutOfBounds(l_position); - return; // Dont process any other motion? - } // end various methods - } // end outside region horizontally - */ + + if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y ) @@ -3146,41 +3474,11 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); _position.Z = Util.Clip(l_position.Z, -100f, 50000f); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - /* - if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) - { // tell base code only once - Interlocked.Increment(ref m_crossingfailures); - base.RequestPhysicsterseUpdate(); - } - */ m_outofBounds = true; base.RequestPhysicsterseUpdate(); return; } -/* - if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) - { - // main simulator had a crossing failure - // park it inside region - _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); - _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); - _position.Z = Util.Clip(l_position.Z, -100f, 50000f); - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - - m_lastposition = _position; - _velocity = Vector3.Zero; - m_lastVelocity = _velocity; - - - if (m_type != Vehicle.TYPE_NONE) - Halt(); - - d.BodySetLinearVel(Body, 0, 0, 0); - base.RequestPhysicsterseUpdate(); - return; - } -*/ base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) @@ -3193,8 +3491,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //IsPhysical = false; if (_parent == null) base.RaiseOutOfBounds(_position); - - + + _acceleration.X = 0; // This stuff may stop client display but it has no _acceleration.Y = 0; // effect on the object in phys engine! _acceleration.Z = 0; @@ -3215,13 +3513,13 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //outofBounds = true; } // end neg Z check - // Is it moving? - /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) - && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) - && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ - if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly + // Is it moving? + /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ + if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating - (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly + (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly { _zeroFlag = true; m_throttleUpdates = false; @@ -3238,18 +3536,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { // Its stopped _velocity.X = 0.0f; _velocity.Y = 0.0f; - // _velocity.Z = 0.0f; + // _velocity.Z = 0.0f; _acceleration.X = 0; _acceleration.Y = 0; - // _acceleration.Z = 0; - + // _acceleration.Z = 0; + m_rotationalVelocity.X = 0; m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; // Stop it in the phys engine - d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); d.BodySetForce(Body, 0f, 0f, 0f); if (!m_lastUpdateSent) @@ -3287,71 +3585,71 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } } m_lastposition = l_position; - - /// End UpdatePositionAndVelocity insert - - + + /// End UpdatePositionAndVelocity insert + + // Rotation lock ===================================== - if(m_rotateEnableUpdate) - { + if (m_rotateEnableUpdate) + { // Snapshot current angles, set up Amotor(s) m_rotateEnableUpdate = false; m_rotateEnable = m_rotateEnableRequest; -//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); + //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); if (Amotor != IntPtr.Zero) { d.JointDestroy(Amotor); Amotor = IntPtr.Zero; -//Console.WriteLine("Old Amotor Destroyed"); + //Console.WriteLine("Old Amotor Destroyed"); } - + if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) { // not all are enabled d.Quaternion r = d.BodyGetQuaternion(Body); Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); // extract the axes vectors - Vector3 vX = new Vector3(1f,0f,0f); - Vector3 vY = new Vector3(0f,1f,0f); - Vector3 vZ = new Vector3(0f,0f,1f); - vX = vX * locrot; + Vector3 vX = new Vector3(1f, 0f, 0f); + Vector3 vY = new Vector3(0f, 1f, 0f); + Vector3 vZ = new Vector3(0f, 0f, 1f); + vX = vX * locrot; vY = vY * locrot; vZ = vZ * locrot; // snapshot the current angle vectors m_lockX = vX; m_lockY = vY; m_lockZ = vZ; - // m_lockRot = locrot; + // m_lockRot = locrot; Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); d.JointAttach(Amotor, Body, IntPtr.Zero); d.JointSetAMotorMode(Amotor, 0); // User mode?? -//Console.WriteLine("New Amotor Created for {0}", m_primName); - + //Console.WriteLine("New Amotor Created for {0}", m_primName); + float axisnum = 3; // how many to lock axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); - d.JointSetAMotorNumAxes(Amotor,(int)axisnum); -//Console.WriteLine("AxisNum={0}",(int)axisnum); + d.JointSetAMotorNumAxes(Amotor, (int)axisnum); + //Console.WriteLine("AxisNum={0}",(int)axisnum); int i = 0; if (m_rotateEnable.X == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); -//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); + //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); i++; } if (m_rotateEnable.Y == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); -//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); + //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); i++; } if (m_rotateEnable.Z == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); -//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); + //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); i++; } @@ -3362,519 +3660,519 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); } // else none are locked } // end Rotation Update - - + + // VEHICLE processing ========================================== - if (m_type != Vehicle.TYPE_NONE) - { - // get body attitude - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - Quaternion irotq = Quaternion.Inverse(rotq); - - // VEHICLE Linear Motion - d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame - Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); - m_lLinObjectVel = vel_now * irotq; - if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate - { - if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) - { - float decayfactor = m_linearMotorDecayTimescale/timestep; - Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); - m_lLinMotorDVel -= decayAmount; - } - else - { - float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); - Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; - m_lLinMotorDVel -= decel; - } - if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) - { - m_lLinMotorDVel = Vector3.Zero; - } - - /* else - { - if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; - if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; - if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; - } */ - } // end linear motor decay - - if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - if (m_linearMotorTimescale < 300.0f) - { - Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; - float linfactor = m_linearMotorTimescale/timestep; - Vector3 attackAmount = (attack_error/linfactor) * 1.3f; - m_lLinObjectVel += attackAmount; - } - if (m_linearFrictionTimescale.X < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.X / timestep; - float fricX = m_lLinObjectVel.X / fricfactor; - m_lLinObjectVel.X -= fricX; - } - if (m_linearFrictionTimescale.Y < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.Y / timestep; - float fricY = m_lLinObjectVel.Y / fricfactor; - m_lLinObjectVel.Y -= fricY; - } - if (m_linearFrictionTimescale.Z < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.Z / timestep; - float fricZ = m_lLinObjectVel.Z / fricfactor; - m_lLinObjectVel.Z -= fricZ; - } - } - m_wLinObjectVel = m_lLinObjectVel * rotq; - - // Gravity and Buoyancy - Vector3 grav = Vector3.Zero; - if(m_VehicleBuoyancy < 1.0f) - { - // There is some gravity, make a gravity force vector - // that is applied after object velocity. - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; - grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force - } // else its 1.0, no gravity. - - // Hovering - if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) - { - // We should hover, get the target height - d.Vector3 pos = d.BodyGetPosition(Body); - if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) - { - m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) - { - m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) - { - m_VhoverTargetHeight = m_VhoverHeight; - } - - if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) - { - // If body is aready heigher, use its height as target height - if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; - } - -// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped -// m_VhoverTimescale = 0f; // time to acheive height -// timestep is time since last frame,in secs - float herr0 = pos.Z - m_VhoverTargetHeight; - // Replace Vertical speed with correction figure if significant - if(Math.Abs(herr0) > 0.01f ) - { - //? d.Mass objMass; - //? d.BodyGetMass(Body, out objMass); - m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale); - //KF: m_VhoverEfficiency is not yet implemented - } - else - { - m_wLinObjectVel.Z = 0f; - } - } - else - { // not hovering - if (m_wLinObjectVel.Z == 0f) - { // Gravity rules - m_wLinObjectVel.Z = vel_now.Z; - } // else the motor has it - } - linvel = m_wLinObjectVel; - - // Vehicle Linear Motion done ======================================= - // Apply velocity - d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); - // apply gravity force - d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); -//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); - // end MoveLinear() - - - // MoveAngular - /* - private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - - private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL - private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL - private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL - - private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor - private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body - */ -//if(frcount == 0) Console.WriteLine("MoveAngular "); + if (m_type != Vehicle.TYPE_NONE) + { + // get body attitude + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion irotq = Quaternion.Inverse(rotq); + + // VEHICLE Linear Motion + d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame + Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); + m_lLinObjectVel = vel_now * irotq; + if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate + { + if (Vector3.Mag(m_lLinMotorDVel) < 1.0f) + { + float decayfactor = m_linearMotorDecayTimescale / timestep; + Vector3 decayAmount = (m_lLinMotorDVel / decayfactor); + m_lLinMotorDVel -= decayAmount; + } + else + { + float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); + Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; + m_lLinMotorDVel -= decel; + } + if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_lLinMotorDVel = Vector3.Zero; + } + + /* else + { + if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; + if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; + if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; + } */ + } // end linear motor decay + + if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f))) + { + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); + if (m_linearMotorTimescale < 300.0f) + { + Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; + float linfactor = m_linearMotorTimescale / timestep; + Vector3 attackAmount = (attack_error / linfactor) * 1.3f; + m_lLinObjectVel += attackAmount; + } + if (m_linearFrictionTimescale.X < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.X / timestep; + float fricX = m_lLinObjectVel.X / fricfactor; + m_lLinObjectVel.X -= fricX; + } + if (m_linearFrictionTimescale.Y < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Y / timestep; + float fricY = m_lLinObjectVel.Y / fricfactor; + m_lLinObjectVel.Y -= fricY; + } + if (m_linearFrictionTimescale.Z < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Z / timestep; + float fricZ = m_lLinObjectVel.Z / fricfactor; + m_lLinObjectVel.Z -= fricZ; + } + } + m_wLinObjectVel = m_lLinObjectVel * rotq; + + // Gravity and Buoyancy + Vector3 grav = Vector3.Zero; + if (m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force + } // else its 1.0, no gravity. + + // Hovering + if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; + } + else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + + // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped + // m_VhoverTimescale = 0f; // time to acheive height + // timestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; + // Replace Vertical speed with correction figure if significant + if (Math.Abs(herr0) > 0.01f) + { + //? d.Mass objMass; + //? d.BodyGetMass(Body, out objMass); + m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale); + //KF: m_VhoverEfficiency is not yet implemented + } + else + { + m_wLinObjectVel.Z = 0f; + } + } + else + { // not hovering + if (m_wLinObjectVel.Z == 0f) + { // Gravity rules + m_wLinObjectVel.Z = vel_now.Z; + } // else the motor has it + } + linvel = m_wLinObjectVel; + + // Vehicle Linear Motion done ======================================= + // Apply velocity + d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); + //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); + // end MoveLinear() + + + // MoveAngular + /* + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); - Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); - angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation - -//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); - - // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. - float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); - m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; - // Decay Angular Motor 2. - if (m_angularMotorDecayTimescale < 300.0f) - { - if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) - { - float decayfactor = (m_angularMotorDecayTimescale)/timestep; - Vector3 decayAmount = (m_angularMotorDVel/decayfactor); - m_angularMotorDVel -= decayAmount; - } - else - { - Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; - m_angularMotorDVel -= decel; - } - - if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) - { - m_angularMotorDVel = Vector3.Zero; - } - else - { - if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; - if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; - if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; - } - } // end decay angular motor -//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); - -//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); - - if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { // if motor or object have motion - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - - if (m_angularMotorTimescale < 300.0f) - { - Vector3 attack_error = m_angularMotorDVel - angObjectVel; - float angfactor = m_angularMotorTimescale/timestep; - Vector3 attackAmount = (attack_error/angfactor); - angObjectVel += attackAmount; -//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); -//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); - } - - angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); - angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); - angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); - } // else no signif. motion - -//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); - // Bank section tba - // Deflection section tba -//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); - - - /* // Rotation Axis Disables: - if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) - { - if (m_angularEnable.X == 0) - angObjectVel.X = 0f; - if (m_angularEnable.Y == 0) - angObjectVel.Y = 0f; - if (m_angularEnable.Z == 0) - angObjectVel.Z = 0f; - } - */ - angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation - - // Vertical attractor section - Vector3 vertattr = Vector3.Zero; - - if(m_verticalAttractionTimescale < 300) - { - float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); - // make a vector pointing up - Vector3 verterr = Vector3.Zero; - verterr.Z = 1.0f; - // rotate it to Body Angle - verterr = verterr * rotq; - // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. - // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go - // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. - - if (verterr.Z < 0.0f) - { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to - // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. -//Console.WriteLine("InvertFlip"); - verterr.X = 2.0f - verterr.X; - verterr.Y = 2.0f - verterr.Y; - } - verterr *= 0.5f; - // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) - Vector3 xyav = angObjectVel; - xyav.Z = 0.0f; - if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) - { - // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so - // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. - vertattr.X = verterr.Y; - vertattr.Y = - verterr.X; - vertattr.Z = 0f; -//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); - - // scaling appears better usingsquare-law - float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; - float bounce = 1.0f - damped; - // 0 = crit damp, 1 = bouncy - float oavz = angObjectVel.Z; // retain z velocity - // time-scaled correction, which sums, therefore is bouncy: - angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; - // damped, good @ < 90: - angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); - angObjectVel.Z = oavz; -//if(frcount == 0) Console.WriteLine("VA+"); -//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); - } - else - { - // else error is very small - angObjectVel.X = 0f; - angObjectVel.Y = 0f; -//if(frcount == 0) Console.WriteLine("VA0"); - } - } // else vertical attractor is off -//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); - - - m_lastAngularVelocity = angObjectVel; - // apply Angular Velocity to body - d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); -//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); - - } // end VEHICLES - else - { - // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ - - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - - /// Dynamics Buoyancy - //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. - // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up - // NB Prims in ODE are no subject to global gravity - // This should only affect gravity operations - - float m_mass = CalculateMass(); - // calculate z-force due togravity on object. - fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass - if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. - { - fz = 0; // llMoveToTarget ignores gravity. - // it also ignores mass of object, and any physical resting on it. - // Vector3 m_PIDTarget is where we are going - // float m_PIDTau is time to get there + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL + private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL + + private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor + private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body + */ + //if(frcount == 0) Console.WriteLine("MoveAngular "); + + d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); + Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); + angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation + + //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); + + // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. + float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); + m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; + // Decay Angular Motor 2. + if (m_angularMotorDecayTimescale < 300.0f) + { + if (Vector3.Mag(m_angularMotorDVel) < 1.0f) + { + float decayfactor = (m_angularMotorDecayTimescale) / timestep; + Vector3 decayAmount = (m_angularMotorDVel / decayfactor); + m_angularMotorDVel -= decayAmount; + } + else + { + Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; + m_angularMotorDVel -= decel; + } + + if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_angularMotorDVel = Vector3.Zero; + } + else + { + if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; + if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; + if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; + } + } // end decay angular motor + //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); + + //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); + + if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f))) + { // if motor or object have motion + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); + + if (m_angularMotorTimescale < 300.0f) + { + Vector3 attack_error = m_angularMotorDVel - angObjectVel; + float angfactor = m_angularMotorTimescale / timestep; + Vector3 attackAmount = (attack_error / angfactor); + angObjectVel += attackAmount; + //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); + //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); + } + + angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); + angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); + angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); + } // else no signif. motion + + //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); + // Bank section tba + // Deflection section tba + //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); + + + /* // Rotation Axis Disables: + if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) + { + if (m_angularEnable.X == 0) + angObjectVel.X = 0f; + if (m_angularEnable.Y == 0) + angObjectVel.Y = 0f; + if (m_angularEnable.Z == 0) + angObjectVel.Z = 0f; + } + */ + angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if (m_verticalAttractionTimescale < 300) + { + float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + + if (verterr.Z < 0.0f) + { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to + // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. + //Console.WriteLine("InvertFlip"); + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + verterr *= 0.5f; + // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) + Vector3 xyav = angObjectVel; + xyav.Z = 0.0f; + if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) + { + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = -verterr.X; + vertattr.Z = 0f; + //if(frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // scaling appears better usingsquare-law + float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; + float bounce = 1.0f - damped; + // 0 = crit damp, 1 = bouncy + float oavz = angObjectVel.Z; // retain z velocity + // time-scaled correction, which sums, therefore is bouncy: + angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; + // damped, good @ < 90: + angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); + angObjectVel.Z = oavz; + //if(frcount == 0) Console.WriteLine("VA+"); + //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); + } + else + { + // else error is very small + angObjectVel.X = 0f; + angObjectVel.Y = 0f; + //if(frcount == 0) Console.WriteLine("VA0"); + } + } // else vertical attractor is off + //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); + + + m_lastAngularVelocity = angObjectVel; + // apply Angular Velocity to body + d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); + //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); + + } // end VEHICLES + else + { + // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ + + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + /// Dynamics Buoyancy + //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // NB Prims in ODE are no subject to global gravity + // This should only affect gravity operations + + float m_mass = CalculateMass(); + // calculate z-force due togravity on object. + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass + if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. + { + fz = 0; // llMoveToTarget ignores gravity. + // it also ignores mass of object, and any physical resting on it. + // Vector3 m_PIDTarget is where we are going + // float m_PIDTau is time to get there fx = 0; - fy = 0; - d.Vector3 pos = d.BodyGetPosition(Body); - Vector3 error = new Vector3( - (m_PIDTarget.X - pos.X), - (m_PIDTarget.Y - pos.Y), - (m_PIDTarget.Z - pos.Z)); - if (error.ApproxEquals(Vector3.Zero,0.01f)) - { // Very close, Jump there and quit move - - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - _target_velocity = Vector3.Zero; + fy = 0; + d.Vector3 pos = d.BodyGetPosition(Body); + Vector3 error = new Vector3( + (m_PIDTarget.X - pos.X), + (m_PIDTarget.Y - pos.Y), + (m_PIDTarget.Z - pos.Z)); + if (error.ApproxEquals(Vector3.Zero, 0.01f)) + { // Very close, Jump there and quit move + + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + _target_velocity = Vector3.Zero; d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); d.BodySetForce(Body, 0f, 0f, 0f); - } - else - { - float scale = 50.0f * timestep / m_PIDTau; - if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero)) - { - // Nearby, quit update of velocity - } - else - { // Far, calc damped velocity - _target_velocity = error * scale; - } - d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); } - } // end PID MoveToTarget - - - /// Dynamics Hover =================================================================================== - // Hover PID Controller can only run if the PIDcontroller is not in use. - if (m_useHoverPID && !m_usePID) - { -//Console.WriteLine("Hover " + m_primName); - - // If we're using the PID controller, then we have no gravity - fz = (-1 * _parent_scene.gravityz) * m_mass; - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); -// d.Vector3 vel = d.BodyGetLinearVel(Body); - - - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; - - } // end switch (m_PIDHoverType) - - - _target_velocity = + else + { + float scale = 50.0f * timestep / m_PIDTau; + if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero)) + { + // Nearby, quit update of velocity + } + else + { // Far, calc damped velocity + _target_velocity = error * scale; + } + d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + } // end PID MoveToTarget + + + /// Dynamics Hover =================================================================================== + // Hover PID Controller can only run if the PIDcontroller is not in use. + if (m_useHoverPID && !m_usePID) + { + //Console.WriteLine("Hover " + m_primName); + + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + // d.Vector3 vel = d.BodyGetLinearVel(Body); + + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - d.Vector3 dlinvel = vel; - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); - d.BodyAddForce(Body, 0, 0, fz); - //KF this prevents furthur motions return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end m_useHoverPID && !m_usePID - - - /// Dynamics Apply Forces =================================================================================== - fx *= m_mass; - fy *= m_mass; - //fz *= m_mass; - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - //m_taintdisable = true; - //base.RaiseOutOfBounds(Position); - //d.BodySetLinearVel(Body, fx, fy, 0f); - if (!d.BodyIsEnabled(Body)) - { - // A physical body at rest on a surface will auto-disable after a while, - // this appears to re-enable it incase the surface it is upon vanishes, - // and the body should fall again. - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0f, 0f, 0f); - enableBodySoft(); - } - - // 35x10 = 350n times the mass per second applied maximum. - float nmax = 35f * m_mass; - float nmin = -35f * m_mass; - - - if (fx > nmax) - fx = nmax; - if (fx < nmin) - fx = nmin; - if (fy > nmax) - fy = nmax; - if (fy < nmin) - fy = nmin; - d.BodyAddForce(Body, fx, fy, fz); - } // end apply forces - } // end Vehicle/Dynamics - - /// RotLookAt / LookAt ================================================================================= + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + d.Vector3 dlinvel = vel; + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); + d.BodyAddForce(Body, 0, 0, fz); + //KF this prevents furthur motions return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end m_useHoverPID && !m_usePID + + + /// Dynamics Apply Forces =================================================================================== + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + //m_taintdisable = true; + //base.RaiseOutOfBounds(Position); + //d.BodySetLinearVel(Body, fx, fy, 0f); + if (!d.BodyIsEnabled(Body)) + { + // A physical body at rest on a surface will auto-disable after a while, + // this appears to re-enable it incase the surface it is upon vanishes, + // and the body should fall again. + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0f, 0f, 0f); + enableBodySoft(); + } + + // 35x10 = 350n times the mass per second applied maximum. + float nmax = 35f * m_mass; + float nmin = -35f * m_mass; + + + if (fx > nmax) + fx = nmax; + if (fx < nmin) + fx = nmin; + if (fy > nmax) + fy = nmax; + if (fy < nmin) + fy = nmin; + d.BodyAddForce(Body, fx, fy, fz); + } // end apply forces + } // end Vehicle/Dynamics + + /// RotLookAt / LookAt ================================================================================= if (m_useAPID) { - // RotLookAt, apparently overrides all other rotation sources. Inputs: - // Quaternion m_APIDTarget - // float m_APIDStrength // From SL experiments, this is the time to get there - // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly - // Also in SL the mass of the object has no effect on time to get there. - // Factors: - // get present body rotation - float limit = 1.0f; - float rscaler = 50f; // adjusts rotation damping time - float lscaler = 10f; // adjusts linear damping time in llLookAt - float RLAservo = 0f; + // RotLookAt, apparently overrides all other rotation sources. Inputs: + // Quaternion m_APIDTarget + // float m_APIDStrength // From SL experiments, this is the time to get there + // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly + // Also in SL the mass of the object has no effect on time to get there. + // Factors: + // get present body rotation + float limit = 1.0f; + float rscaler = 50f; // adjusts rotation damping time + float lscaler = 10f; // adjusts linear damping time in llLookAt + float RLAservo = 0f; Vector3 diff_axis; float diff_angle; - d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Quaternion rtarget = new Quaternion(); - - if(m_APIDTarget.W == -99.9f) - { - // this is really a llLookAt(), x,y,z is the target vector - Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); - Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; + d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rtarget = new Quaternion(); + + if (m_APIDTarget.W == -99.9f) + { + // this is really a llLookAt(), x,y,z is the target vector + Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); + Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; Vector3 error = new Vector3(0.0f, 0.0f, 0.0f); float twopi = 2.0f * (float)Math.PI; - Vector3 dir = target - _position; - dir.Normalize(); + Vector3 dir = target - _position; + dir.Normalize(); float tzrot = (float)Math.Atan2(dir.Y, dir.X); float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y)); float terot = (float)Math.Atan2(dir.Z, txy); @@ -3882,63 +4180,63 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y)); float oerot = (float)Math.Atan2(ospin.Z, oxy); float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z)); - float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X)); + float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X)); float roll = (float)Math.Atan2(ra, rb); float errorz = tzrot - ozrot; - if(errorz > (float)Math.PI) errorz -= twopi; - else if(errorz < -(float)Math.PI) errorz += twopi; + if (errorz > (float)Math.PI) errorz -= twopi; + else if (errorz < -(float)Math.PI) errorz += twopi; float errory = oerot - terot; - if(errory > (float)Math.PI) errory -= twopi; - else if(errory < -(float)Math.PI) errory += twopi; + if (errory > (float)Math.PI) errory -= twopi; + else if (errory < -(float)Math.PI) errory += twopi; diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll); - if(diff_angle > 0.01f * m_APIDdamper) - { - m_APIDdamper = 1.0f; - RLAservo = timestep / m_APIDStrength * rscaler; + if (diff_angle > 0.01f * m_APIDdamper) + { + m_APIDdamper = 1.0f; + RLAservo = timestep / m_APIDStrength * rscaler; errorz *= RLAservo; errory *= RLAservo; error.X = -roll * 8.0f; error.Y = errory; error.Z = errorz; error *= rotq; - d.BodySetAngularVel (Body, error.X, error.Y, error.Z); - } - else - { - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - m_APIDdamper = 2.0f; - } - } - else - { - // this is a llRotLookAt() - rtarget = m_APIDTarget; - - Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot - rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle -//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); - - // diff_axis.Normalize(); it already is! - if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error - { - m_APIDdamper = 1.0f; - Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); - rotforce = rotforce * rotq; - if(diff_angle > limit) diff_angle = limit; // cap the rotate rate - RLAservo = timestep / m_APIDStrength * lscaler; - rotforce = rotforce * RLAservo * diff_angle ; - d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); -//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); - } - else - { // close enough - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - m_APIDdamper = 2.0f; - } - } // end llLookAt/llRotLookAt -//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); + d.BodySetAngularVel(Body, error.X, error.Y, error.Z); + } + else + { + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + m_APIDdamper = 2.0f; + } + } + else + { + // this is a llRotLookAt() + rtarget = m_APIDTarget; + + Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle + //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); + + // diff_axis.Normalize(); it already is! + if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error + { + m_APIDdamper = 1.0f; + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if (diff_angle > limit) diff_angle = limit; // cap the rotate rate + RLAservo = timestep / m_APIDStrength * lscaler; + rotforce = rotforce * RLAservo * diff_angle; + d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z); + //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } + else + { // close enough + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + m_APIDdamper = 2.0f; + } + } // end llLookAt/llRotLookAt + //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); } // end m_useAPID } // end root prims - } // end Move() - } // end class -} + } // end Move() + } // end class +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index cf7c1d7..03059f7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1736,6 +1736,23 @@ namespace OpenSim.Region.Physics.OdePlugin return newPrim; } + private PhysicsActor AddPrim(String name, Vector3 position, PhysicsActor parent, + PrimitiveBaseShape pbs, uint localid, byte[] sdata) + { + Vector3 pos = position; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, parent, pbs, ode, localid, sdata); + lock (_prims) + _prims.Add(newPrim); + } + + return newPrim; + } + + public void addActivePrim(OdePrim activatePrim) { // adds active prim.. (ones that should be iterated over in collisions_optimized @@ -1762,6 +1779,17 @@ namespace OpenSim.Region.Physics.OdePlugin return result; } + public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, + uint localid, byte[] sdata) + { + PhysicsActor result; + + result = AddPrim(primName, position, parent, + pbs, localid, sdata); + + return result; + } + public override float TimeDilation { get { return m_timeDilation; } diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs new file mode 100644 index 0000000..edd58d3 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs @@ -0,0 +1,167 @@ +// adapted from libomv removing cpu endian adjust +// for prims lowlevel serialization + +using System; +using System.IO; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class wstreamer + { + private MemoryStream st; + + public wstreamer() + { + st = new MemoryStream(); + } + + public byte[] close() + { + byte[] data = st.ToArray(); + st.Close(); + return data; + } + + public void Wshort(short value) + { + st.Write(BitConverter.GetBytes(value), 0, 2); + } + public void Wushort(ushort value) + { + byte[] t = BitConverter.GetBytes(value); + st.Write(BitConverter.GetBytes(value), 0, 2); + } + public void Wint(int value) + { + st.Write(BitConverter.GetBytes(value), 0, 4); + } + public void Wuint(uint value) + { + st.Write(BitConverter.GetBytes(value), 0, 4); + } + public void Wlong(long value) + { + st.Write(BitConverter.GetBytes(value), 0, 8); + } + public void Wulong(ulong value) + { + st.Write(BitConverter.GetBytes(value), 0, 8); + } + + public void Wfloat(float value) + { + st.Write(BitConverter.GetBytes(value), 0, 4); + } + + public void Wdouble(double value) + { + st.Write(BitConverter.GetBytes(value), 0, 8); + } + + public void Wvector3(Vector3 value) + { + st.Write(BitConverter.GetBytes(value.X), 0, 4); + st.Write(BitConverter.GetBytes(value.Y), 0, 4); + st.Write(BitConverter.GetBytes(value.Z), 0, 4); + } + public void Wquat(Quaternion value) + { + st.Write(BitConverter.GetBytes(value.X), 0, 4); + st.Write(BitConverter.GetBytes(value.Y), 0, 4); + st.Write(BitConverter.GetBytes(value.Z), 0, 4); + st.Write(BitConverter.GetBytes(value.W), 0, 4); + } + } + + public class rstreamer + { + private byte[] rbuf; + private int ptr; + + public rstreamer(byte[] data) + { + rbuf = data; + ptr = 0; + } + + public void close() + { + } + + public short Rshort() + { + short v = BitConverter.ToInt16(rbuf, ptr); + ptr += 2; + return v; + } + public ushort Rushort() + { + ushort v = BitConverter.ToUInt16(rbuf, ptr); + ptr += 2; + return v; + } + public int Rint() + { + int v = BitConverter.ToInt32(rbuf, ptr); + ptr += 4; + return v; + } + public uint Ruint() + { + uint v = BitConverter.ToUInt32(rbuf, ptr); + ptr += 4; + return v; + } + public long Rlong() + { + long v = BitConverter.ToInt64(rbuf, ptr); + ptr += 8; + return v; + } + public ulong Rulong() + { + ulong v = BitConverter.ToUInt64(rbuf, ptr); + ptr += 8; + return v; + } + public float Rfloat() + { + float v = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + return v; + } + + public double Rdouble() + { + double v = BitConverter.ToDouble(rbuf, ptr); + ptr += 8; + return v; + } + + public Vector3 Rvector3() + { + Vector3 v; + v.X = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.Y = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.Z = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + return v; + } + public Quaternion Rquat() + { + Quaternion v; + v.X = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.Y = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.Z = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.W = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + return v; + } + } +} diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index e1a68be..eb0228a 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -214,6 +214,11 @@ namespace OpenSim.Region.Physics.Manager } } + public virtual byte[] Serialize(bool PhysIsRunning) + { + return new byte[0]; + } + public virtual void RaiseOutOfBounds(Vector3 pos) { // Make a temporary copy of the event to avoid possibility of @@ -573,5 +578,6 @@ namespace OpenSim.Region.Physics.Manager { return false; } + } } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 3db71e5..0346d4e 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -128,6 +128,12 @@ namespace OpenSim.Region.Physics.Manager public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid); + public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, + uint localid, byte[] sdata) + { + return null; + } + public virtual float TimeDilation { get { return 1.0f; } -- cgit v1.1 From 3aee642190add7045f78e522ae7b2221b3566f1e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 18 Feb 2012 17:42:14 +0000 Subject: changed how vehicle data is stored and passed to physics. use unsafe in serializer, tried to control m_dupeInProgress --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 62 ++++- OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs | 294 +++++++++++++++++---- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 5 +- OpenSim/Region/Physics/Manager/VehicleConstants.cs | 45 +++- 4 files changed, 347 insertions(+), 59 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 6e28bfa..2306309 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -22,7 +22,8 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * - * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs + * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs + * Ubit 2012 * rolled into ODEPrim.cs */ @@ -38,7 +39,6 @@ using Ode.NET; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; - namespace OpenSim.Region.Physics.OdePlugin { /// @@ -254,6 +254,61 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. SerialControl m_taintserial = null; + object m_taintvehicledata = null; + + public void DoSetVehicle() + { + VehicleData vd = (VehicleData)m_taintvehicledata; + + m_type = vd.m_type; + m_flags = vd.m_flags; + + // Linear properties + m_linearMotorDirection = vd.m_linearMotorDirection; + m_linearFrictionTimescale = vd.m_linearFrictionTimescale; + m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; + m_linearMotorTimescale = vd.m_linearMotorTimescale; +// m_linearMotorOffset = vd.m_linearMotorOffset; + + //Angular properties + m_angularMotorDirection = vd.m_angularMotorDirection; + m_angularMotorTimescale = vd.m_angularMotorTimescale; + m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; + m_angularFrictionTimescale = vd.m_angularFrictionTimescale; + + //Deflection properties +// m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; +// m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; +// m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; +// m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; + + //Banking properties +// m_bankingEfficiency = vd.m_bankingEfficiency; +// m_bankingMix = vd.m_bankingMix; +// m_bankingTimescale = vd.m_bankingTimescale; + + //Hover and Buoyancy properties + m_VhoverHeight = vd.m_VhoverHeight; +// m_VhoverEfficiency = vd.m_VhoverEfficiency; + m_VhoverTimescale = vd.m_VhoverTimescale; + m_VehicleBuoyancy = vd.m_VehicleBuoyancy; + + //Attractor properties + m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; + m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; + + // Axis +// m_referenceFrame = vd.m_referenceFrame; + + + m_taintvehicledata = null; + } + + public override void SetVehicle(object vdata) + { + m_taintvehicledata = vdata; + _parent_scene.AddPhysicsActorTaint(this); + } public override byte[] Serialize(bool PhysIsRunning) { @@ -1843,6 +1898,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); + if (m_taintvehicledata != null) + DoSetVehicle(); + if (m_taintserial != null) DoSerialize(m_taintserial); diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs index edd58d3..e7e7bb3 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs @@ -1,5 +1,17 @@ -// adapted from libomv removing cpu endian adjust -// for prims lowlevel serialization +/* Ubit 2012 + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +// no endian conversion. So can't be use to pass information around diferent cpus with diferent endian using System; using System.IO; @@ -7,77 +19,168 @@ using OpenMetaverse; namespace OpenSim.Region.Physics.OdePlugin { - public class wstreamer + + unsafe public class wstreamer { - private MemoryStream st; + byte[] buf; + int index; + byte* src; public wstreamer() { - st = new MemoryStream(); + buf = new byte[1024]; + index = 0; + } + public wstreamer(int size) + { + buf = new byte[size]; + index = 0; } public byte[] close() { - byte[] data = st.ToArray(); - st.Close(); + byte[] data = new byte[index]; + Buffer.BlockCopy(buf, 0, data, 0, index); return data; } + public void Seek(int pos) + { + index = pos; + } + + public void Seekrel(int pos) + { + index += pos; + } + + public void Wbyte(byte value) + { + buf[index++] = value; + } public void Wshort(short value) { - st.Write(BitConverter.GetBytes(value), 0, 2); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src; } public void Wushort(ushort value) { - byte[] t = BitConverter.GetBytes(value); - st.Write(BitConverter.GetBytes(value), 0, 2); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src; } public void Wint(int value) { - st.Write(BitConverter.GetBytes(value), 0, 4); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wuint(uint value) { - st.Write(BitConverter.GetBytes(value), 0, 4); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wlong(long value) { - st.Write(BitConverter.GetBytes(value), 0, 8); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wulong(ulong value) { - st.Write(BitConverter.GetBytes(value), 0, 8); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wfloat(float value) { - st.Write(BitConverter.GetBytes(value), 0, 4); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wdouble(double value) { - st.Write(BitConverter.GetBytes(value), 0, 8); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wvector3(Vector3 value) { - st.Write(BitConverter.GetBytes(value.X), 0, 4); - st.Write(BitConverter.GetBytes(value.Y), 0, 4); - st.Write(BitConverter.GetBytes(value.Z), 0, 4); + src = (byte*)&value.X; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Y; // it may have padding ?? + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Z; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wquat(Quaternion value) { - st.Write(BitConverter.GetBytes(value.X), 0, 4); - st.Write(BitConverter.GetBytes(value.Y), 0, 4); - st.Write(BitConverter.GetBytes(value.Z), 0, 4); - st.Write(BitConverter.GetBytes(value.W), 0, 4); + src = (byte*)&value.X; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Y; // it may have padding ?? + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Z; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.W; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } } - public class rstreamer + unsafe public class rstreamer { private byte[] rbuf; private int ptr; + private byte* dst; public rstreamer(byte[] data) { @@ -89,78 +192,161 @@ namespace OpenSim.Region.Physics.OdePlugin { } + public void Seek(int pos) + { + ptr = pos; + } + + public void Seekrel(int pos) + { + ptr += pos; + } + + public byte Rbyte() + { + return (byte)rbuf[ptr++]; + } + public short Rshort() { - short v = BitConverter.ToInt16(rbuf, ptr); - ptr += 2; + short v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public ushort Rushort() { - ushort v = BitConverter.ToUInt16(rbuf, ptr); - ptr += 2; + ushort v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public int Rint() { - int v = BitConverter.ToInt32(rbuf, ptr); - ptr += 4; + int v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public uint Ruint() { - uint v = BitConverter.ToUInt32(rbuf, ptr); - ptr += 4; + uint v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public long Rlong() { - long v = BitConverter.ToInt64(rbuf, ptr); - ptr += 8; + long v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public ulong Rulong() { - ulong v = BitConverter.ToUInt64(rbuf, ptr); - ptr += 8; + ulong v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public float Rfloat() { - float v = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; + float v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public double Rdouble() { - double v = BitConverter.ToDouble(rbuf, ptr); - ptr += 8; + double v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public Vector3 Rvector3() { Vector3 v; - v.X = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.Y = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.Z = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; + dst = (byte*)&v.X; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Y; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Z; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } + public Quaternion Rquat() { Quaternion v; - v.X = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.Y = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.Z = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.W = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; + dst = (byte*)&v.X; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Y; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Z; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.W; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; } } diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index eb0228a..f525e9e 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -146,6 +146,8 @@ namespace OpenSim.Region.Physics.Manager /// public event CollisionUpdate OnCollisionUpdate; + public virtual void SetVehicle(object vdata) { } + public event OutOfBounds OnOutOfBounds; #pragma warning restore 67 @@ -153,8 +155,7 @@ namespace OpenSim.Region.Physics.Manager { get { return new NullPhysicsActor(); } } - - + public virtual bool Building { get; set; } public virtual ContactData ContactData diff --git a/OpenSim/Region/Physics/Manager/VehicleConstants.cs b/OpenSim/Region/Physics/Manager/VehicleConstants.cs index f0775c1..8e24b4c 100644 --- a/OpenSim/Region/Physics/Manager/VehicleConstants.cs +++ b/OpenSim/Region/Physics/Manager/VehicleConstants.cs @@ -26,6 +26,7 @@ */ using System; +using OpenMetaverse; namespace OpenSim.Region.Physics.Manager { @@ -117,5 +118,47 @@ namespace OpenSim.Region.Physics.Manager NO_DEFLECTION = 16392, LOCK_ROTATION = 32784 } - + + public struct VehicleData + { + public Vehicle m_type; + public VehicleFlag m_flags; + + // Linear properties + public Vector3 m_linearMotorDirection; + public Vector3 m_linearFrictionTimescale; + public float m_linearMotorDecayTimescale; + public float m_linearMotorTimescale; + public Vector3 m_linearMotorOffset; + + //Angular properties + public Vector3 m_angularMotorDirection; + public float m_angularMotorTimescale; + public float m_angularMotorDecayTimescale; + public Vector3 m_angularFrictionTimescale; + + //Deflection properties + public float m_angularDeflectionEfficiency; + public float m_angularDeflectionTimescale; + public float m_linearDeflectionEfficiency; + public float m_linearDeflectionTimescale; + + //Banking properties + public float m_bankingEfficiency; + public float m_bankingMix; + public float m_bankingTimescale; + + //Hover and Buoyancy properties + public float m_VhoverHeight; + public float m_VhoverEfficiency; + public float m_VhoverTimescale; + public float m_VehicleBuoyancy; + + //Attractor properties + public float m_verticalAttractionEfficiency; + public float m_verticalAttractionTimescale; + + // Axis + public Quaternion m_referenceFrame; + } } -- cgit v1.1 From 736fb0b41dae9ab4a66e556bbb37aeb9d6360f1d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 19 Feb 2012 02:07:43 +0000 Subject: minor fix to chODE terrain heighmap scale --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 03059f7..b5b30ee 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -3440,15 +3440,15 @@ namespace OpenSim.Region.Physics.OdePlugin int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M) - int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra + int heightmapWidth = regionsize + 2; // ODE map size 258 x 258 (Meters) (1 extra each side) int heightmapHeight = regionsize + 2; - int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples - int heightmapHeightSamples = (int)regionsize + 2; + int heightmapWidthSamples = (int)regionsize + 3; // to have 258m we need 259 samples + int heightmapHeightSamples = (int)regionsize + 3; // Array of height samples for ODE float[] _heightmap; - _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258 + _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 259 x 259 // Other ODE parameters const float scale = 1.0f; -- cgit v1.1 From cb4509f3b8c88c1cfa1a46328661d58abe714db6 Mon Sep 17 00:00:00 2001 From: Melanie Date: Tue, 21 Feb 2012 21:34:02 +0100 Subject: Reverse the changed to ODE heightmap. It results in SERIOUS issues like an irreversible y-flip and holes in the map. --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index b5b30ee..03059f7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -3440,15 +3440,15 @@ namespace OpenSim.Region.Physics.OdePlugin int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M) - int heightmapWidth = regionsize + 2; // ODE map size 258 x 258 (Meters) (1 extra each side) + int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra int heightmapHeight = regionsize + 2; - int heightmapWidthSamples = (int)regionsize + 3; // to have 258m we need 259 samples - int heightmapHeightSamples = (int)regionsize + 3; + int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples + int heightmapHeightSamples = (int)regionsize + 2; // Array of height samples for ODE float[] _heightmap; - _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 259 x 259 + _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258 // Other ODE parameters const float scale = 1.0f; -- cgit v1.1 From aa77d1d486f11da7dc841190f1ca85e085d0d648 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 23 Feb 2012 00:22:44 +0000 Subject: fix my bug on ChODE terrain heightmap build --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 03059f7..e73454f 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -3443,8 +3443,8 @@ namespace OpenSim.Region.Physics.OdePlugin int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra int heightmapHeight = regionsize + 2; - int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples - int heightmapHeightSamples = (int)regionsize + 2; + int heightmapWidthSamples = (int)regionsize + 3; // Sample file size, 258 x 258 samples + int heightmapHeightSamples = (int)regionsize + 3; // Array of height samples for ODE float[] _heightmap; @@ -3481,7 +3481,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Output x = 0 1 2 3 ..... 255 256 257 258 total out float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255> if (val < minele) val = minele; - _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> + _heightmap[x * (heightmapWidthSamples) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> hfmin = (val < hfmin) ? val : hfmin; hfmax = (val > hfmax) ? val : hfmax; } -- cgit v1.1 From 15bc539bd49e7a09c1ec6e539871cde5eee6032e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 23 Feb 2012 00:50:07 +0000 Subject: fix the last fix. Regions are square but... Also remove the 0.5 offset in map position. It was apparently needed to fix we having nsamples = size and not size + 1. --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index e73454f..61fb2d0 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -3481,7 +3481,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Output x = 0 1 2 3 ..... 255 256 257 258 total out float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255> if (val < minele) val = minele; - _heightmap[x * (heightmapWidthSamples) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> + _heightmap[x * (heightmapHeightSamples) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> hfmin = (val < hfmin) ? val : hfmin; hfmax = (val > hfmax) ? val : hfmax; } @@ -3531,6 +3531,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0); + // having nsamples = size + 1 center is actually at size/2 + d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)), (pOffset.Y + (regionsize * 0.5f)), 0); IntPtr testGround = IntPtr.Zero; if (RegionTerrain.TryGetValue(pOffset, out testGround)) { -- cgit v1.1 From ec94b82f514b988ad9b931c842a022a0fb5b3bae Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 24 Feb 2012 18:25:43 +0000 Subject: stop using useless geom tricallback. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 80c1277..41afe76 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -1810,7 +1810,8 @@ namespace OpenSim.Region.Physics.OdePlugin { // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer // { - SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); + // SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); // } } catch (AccessViolationException) -- cgit v1.1 From 23679c38085ea12f9845b3daa5edc72c204e583c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 24 Feb 2012 20:34:11 +0000 Subject: replace bad meshs by a small cube and log it. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 41afe76..636039b 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -1762,7 +1762,7 @@ namespace OpenSim.Region.Physics.OdePlugin private static Dictionary m_MeshToTriMeshMap = new Dictionary(); - public void setMesh(OdeScene parent_scene, IMesh mesh) + public bool setMesh(OdeScene parent_scene, IMesh mesh) { // This sleeper is there to moderate how long it takes between // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object @@ -1791,18 +1791,30 @@ namespace OpenSim.Region.Physics.OdePlugin mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z); + _size.X = 0.01f; + _size.Y = 0.01f; + _size.Z = 0.01f; + return false; + } + + +/* mesh.releaseSourceMeshData(); // free up the original mesh data to save memory if (m_MeshToTriMeshMap.ContainsKey(mesh)) { _triMeshData = m_MeshToTriMeshMap[mesh]; } else + */ { _triMeshData = d.GeomTriMeshDataCreate(); d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); d.GeomTriMeshDataPreprocess(_triMeshData); - m_MeshToTriMeshMap[mesh] = _triMeshData; +// m_MeshToTriMeshMap[mesh] = _triMeshData; } _parent_scene.waitForSpaceUnlock(m_targetSpace); @@ -1817,7 +1829,7 @@ namespace OpenSim.Region.Physics.OdePlugin catch (AccessViolationException) { m_log.Error("[PHYSICS]: MESH LOCKED"); - return; + return false; } @@ -1829,6 +1841,7 @@ namespace OpenSim.Region.Physics.OdePlugin // enableBody(); // } + return true; } public void ProcessTaints(float timestep) //============================================================================= @@ -2287,11 +2300,14 @@ namespace OpenSim.Region.Physics.OdePlugin public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) { + bool gottrimesh = false; + if (_mesh != null) // Special - make mesh { - setMesh(_parent_scene, _mesh); + gottrimesh = setMesh(_parent_scene, _mesh); } - else // not a mesh + + if(!gottrimesh) // not a mesh { if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) // special profile?? { -- cgit v1.1 From 118986f15078a1df5561a64355e6f4777ae74fe1 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 26 Feb 2012 17:51:43 +0000 Subject: added in chODE GeomTriMeshDataDestroy to explicity release internal trimesh data. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 33 +++++++++++++++++++++++-- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 11 ++++++++- 2 files changed, 41 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 636039b..86ab58d 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -704,6 +704,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical) m_targetSpace = _parent_scene.space; + _triMeshData = IntPtr.Zero; + m_primName = primName; m_taintserial = null; m_taintadd = true; @@ -773,6 +775,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_targetSpace = _parent_scene.space; } + _triMeshData = IntPtr.Zero; + m_taintserial = null; m_primName = primName; m_taintadd = true; @@ -1785,6 +1789,15 @@ namespace OpenSim.Region.Physics.OdePlugin disableBody(); } } + +// do it on caller instead +/* + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } +*/ IntPtr vertices, indices; int vertexCount, indexCount; int vertexStride, triStride; @@ -1801,14 +1814,17 @@ namespace OpenSim.Region.Physics.OdePlugin } -/* + // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid mesh.releaseSourceMeshData(); // free up the original mesh data to save memory +/* if (m_MeshToTriMeshMap.ContainsKey(mesh)) { _triMeshData = m_MeshToTriMeshMap[mesh]; } else - */ +*/ + + { _triMeshData = d.GeomTriMeshDataCreate(); @@ -1829,6 +1845,13 @@ namespace OpenSim.Region.Physics.OdePlugin catch (AccessViolationException) { m_log.Error("[PHYSICS]: MESH LOCKED"); + + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + return false; } @@ -2302,6 +2325,12 @@ namespace OpenSim.Region.Physics.OdePlugin { bool gottrimesh = false; + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + if (_mesh != null) // Special - make mesh { gottrimesh = setMesh(_parent_scene, _mesh); diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 61fb2d0..f84918c 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -2174,6 +2174,16 @@ namespace OpenSim.Region.Physics.OdePlugin { prim.ResetTaints(); + try + { + if (prim._triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(prim._triMeshData); + prim._triMeshData = IntPtr.Zero; + } + } + catch { }; + if (prim.IsPhysical) { prim.disableBody(); @@ -2185,7 +2195,6 @@ namespace OpenSim.Region.Physics.OdePlugin prim.IsPhysical = false; } - } // we don't want to remove the main space -- cgit v1.1 From 0d51c42f59c1d539a47f9c057469e852a5ff3868 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 27 Feb 2012 02:10:03 +0000 Subject: update ubitODE to my current working state --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 110 ++++- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 321 ++++++------- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 1 - OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 514 +++++---------------- 4 files changed, 378 insertions(+), 568 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 80218e7..c9d0909 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -83,7 +83,6 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); private float m_linearMotorDecayTimescale = 120; private float m_linearMotorTimescale = 1000; - private Vector3 m_lastLinearVelocityVector = Vector3.Zero; private Vector3 m_linearMotorOffset = Vector3.Zero; //Angular properties @@ -91,7 +90,6 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate - private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body //Deflection properties private float m_angularDeflectionEfficiency = 0; @@ -102,7 +100,7 @@ namespace OpenSim.Region.Physics.OdePlugin //Banking properties private float m_bankingEfficiency = 0; private float m_bankingMix = 0; - private float m_bankingTimescale = 0; + private float m_bankingTimescale = 1000; //Hover and Buoyancy properties private float m_VhoverHeight = 0f; @@ -117,9 +115,8 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_verticalAttractionEfficiency = 1.0f; // damped private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. - // auxiliar - private Vector3 m_dir = Vector3.Zero; // velocity applied to body + // auxiliar private float m_lmEfect = 0; // current linear motor eficiency private float m_amEfect = 0; // current angular motor eficiency @@ -130,6 +127,82 @@ namespace OpenSim.Region.Physics.OdePlugin _pParentScene = rootPrim._parent_scene; } + + public void DoSetVehicle(VehicleData vd) + { + + float timestep = _pParentScene.ODE_STEPSIZE; + float invtimestep = 1.0f / timestep; + + m_type = vd.m_type; + m_flags = vd.m_flags; + + // Linear properties + m_linearMotorDirection = vd.m_linearMotorDirection; + + m_linearFrictionTimescale = vd.m_linearFrictionTimescale; + if (m_linearFrictionTimescale.X < timestep) m_linearFrictionTimescale.X = timestep; + if (m_linearFrictionTimescale.Y < timestep) m_linearFrictionTimescale.Y = timestep; + if (m_linearFrictionTimescale.Z < timestep) m_linearFrictionTimescale.Z = timestep; + + m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; + if (m_linearMotorDecayTimescale < 0.5f) m_linearMotorDecayTimescale = 0.5f; + m_linearMotorDecayTimescale *= invtimestep; + + m_linearMotorTimescale = vd.m_linearMotorTimescale; + if (m_linearMotorTimescale < timestep) m_linearMotorTimescale = timestep; + + m_linearMotorOffset = vd.m_linearMotorOffset; + + //Angular properties + m_angularMotorDirection = vd.m_angularMotorDirection; + m_angularMotorTimescale = vd.m_angularMotorTimescale; + if (m_angularMotorTimescale < timestep) m_angularMotorTimescale = timestep; + + m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; + if (m_angularMotorDecayTimescale < 0.5f) m_angularMotorDecayTimescale = 0.5f; + m_angularMotorDecayTimescale *= invtimestep; + + m_angularFrictionTimescale = vd.m_angularFrictionTimescale; + if (m_angularFrictionTimescale.X < timestep) m_angularFrictionTimescale.X = timestep; + if (m_angularFrictionTimescale.Y < timestep) m_angularFrictionTimescale.Y = timestep; + if (m_angularFrictionTimescale.Z < timestep) m_angularFrictionTimescale.Z = timestep; + + //Deflection properties + m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; + m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; + if (m_angularDeflectionTimescale < timestep) m_angularDeflectionTimescale = timestep; + + m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; + m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; + if (m_linearDeflectionTimescale < timestep) m_linearDeflectionTimescale = timestep; + + //Banking properties + m_bankingEfficiency = vd.m_bankingEfficiency; + m_bankingMix = vd.m_bankingMix; + m_bankingTimescale = vd.m_bankingTimescale; + if (m_bankingTimescale < timestep) m_bankingTimescale = timestep; + + //Hover and Buoyancy properties + m_VhoverHeight = vd.m_VhoverHeight; + m_VhoverEfficiency = vd.m_VhoverEfficiency; + m_VhoverTimescale = vd.m_VhoverTimescale; + if (m_VhoverTimescale < timestep) m_VhoverTimescale = timestep; + + m_VehicleBuoyancy = vd.m_VehicleBuoyancy; + + //Attractor properties + m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; + m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; + if (m_verticalAttractionTimescale < timestep) m_verticalAttractionTimescale = timestep; + + // Axis + m_referenceFrame = vd.m_referenceFrame; + + m_lmEfect = 0; + m_amEfect = 0; + } + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) { float len; @@ -231,6 +304,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 12.566f) m_angularMotorDirection *= (12.566f / len); m_amEfect = 1.0f; // turn it on + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: if (pValue < timestep) pValue = timestep; @@ -242,6 +318,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 30.0f) m_linearMotorDirection *= (30.0f / len); m_lmEfect = 1.0f; // turn it on + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); break; case Vehicle.LINEAR_MOTOR_OFFSET: m_linearMotorOffset = new Vector3(pValue, pValue, pValue); @@ -273,6 +352,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 12.566f) m_angularMotorDirection *= (12.566f / len); m_amEfect = 1.0f; // turn it on + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: if (pValue.X < timestep) pValue.X = timestep; @@ -286,6 +368,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 30.0f) m_linearMotorDirection *= (30.0f / len); m_lmEfect = 1.0f; // turn it on + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); break; case Vehicle.LINEAR_MOTOR_OFFSET: m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); @@ -347,12 +432,23 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120 * invtimestep; + m_linearMotorDecayTimescale = 120; m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 1000 * invtimestep; + m_angularMotorDecayTimescale = 1000; m_VhoverHeight = 0; + m_VhoverEfficiency = 1; m_VhoverTimescale = 1000; m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 0; + m_linearDeflectionTimescale = 1000; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 1000; + m_bankingEfficiency = 0; + m_bankingMix = 1; + m_bankingTimescale = 1000; + m_verticalAttractionEfficiency = 0; + m_verticalAttractionTimescale = 1000; + m_flags = (VehicleFlag)0; break; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 3b7f562..0ccdbc0 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -111,7 +111,7 @@ namespace OpenSim.Region.Physics.OdePlugin | CollisionCategories.Body | CollisionCategories.Character ); - private bool m_collidesLand = true; +// private bool m_collidesLand = true; private bool m_collidesWater; public bool m_returnCollisions; @@ -122,7 +122,7 @@ namespace OpenSim.Region.Physics.OdePlugin private CollisionCategories m_collisionFlags = m_default_collisionFlags; public bool m_disabled; - public bool m_taintselected; + public uint m_localID; @@ -142,20 +142,19 @@ namespace OpenSim.Region.Physics.OdePlugin private List childrenPrim = new List(); private bool m_iscolliding; - private bool m_wascolliding; - private bool m_isSelected; + + public bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + public bool m_outbounds; internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively private bool m_throttleUpdates; private int throttleCounter; - public int m_interpenetrationcount; public float m_collisionscore; int m_colliderfilter = 0; - public int m_roundsUnderMotionThreshold; - private int m_crossingfailures; - public bool outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; public bool _zeroFlag; @@ -166,12 +165,11 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 _target_velocity; public Vector3 primOOBsize; // prim real dimensions from mesh - public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb + public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb public float primOOBradiusSQ; public d.Mass primdMass; // prim inertia information on it's own referencial float primMass; // prim own mass float _mass; // object mass acording to case - public d.Mass objectpMass; // object last computed inertia private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb public int givefakepos = 0; @@ -182,9 +180,6 @@ namespace OpenSim.Region.Physics.OdePlugin public int m_eventsubscription; private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); - private IntPtr m_linkJoint = IntPtr.Zero; - private IntPtr _linkJointGroup = IntPtr.Zero; - public volatile bool childPrim; public ODEDynamics m_vehicle; @@ -264,7 +259,7 @@ namespace OpenSim.Region.Physics.OdePlugin set { if (value) - m_isSelected = value; + m_isSelected = value; // if true set imediatly to stop moves etc AddChange(changes.Selected, value); } } @@ -298,13 +293,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_iscolliding = false; else m_iscolliding = true; - - if (m_wascolliding != m_iscolliding) - { - if (m_wascolliding && !m_isSelected && Body != IntPtr.Zero) - d.BodyEnable(Body); - m_wascolliding = m_iscolliding; - } } } @@ -665,19 +653,21 @@ namespace OpenSim.Region.Physics.OdePlugin strVehicleQuatParam fp = new strVehicleQuatParam(); fp.param = param; fp.value = value; - AddChange(changes.VehicleVectorParam, fp); + AddChange(changes.VehicleRotationParam, fp); } public override void VehicleFlags(int param, bool value) { - if (m_vehicle == null) - return; strVehicleBoolParam bp = new strVehicleBoolParam(); bp.param = param; bp.value = value; AddChange(changes.VehicleFlags, bp); } + public override void SetVehicle(object vdata) + { + AddChange(changes.SetVehicle, vdata); + } public void SetAcceleration(Vector3 accel) { _acceleration = accel; @@ -710,8 +700,30 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CrossingFailure() { - m_crossingfailures++; - changeDisable(false); + if (m_outbounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + m_lastposition = _position; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + if(Body != IntPtr.Zero) + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + if (prim_geom != IntPtr.Zero) + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + + m_outbounds = false; + changeDisable(false); + base.RequestPhysicsterseUpdate(); + } } public override void SetMomentum(Vector3 momentum) @@ -865,12 +877,14 @@ namespace OpenSim.Region.Physics.OdePlugin m_force = Vector3.Zero; m_iscolliding = false; - m_wascolliding = false; m_colliderfilter = 0; hasOOBoffsetFromMesh = false; _triMeshData = IntPtr.Zero; + m_lastdoneSelected = false; + m_isSelected = false; + m_delaySelect = false; primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; @@ -885,8 +899,6 @@ namespace OpenSim.Region.Physics.OdePlugin private void resetCollisionAccounting() { m_collisionscore = 0; - m_interpenetrationcount = 0; - m_disabled = false; } private void createAMotor(Vector3 axis) @@ -926,9 +938,6 @@ namespace OpenSim.Region.Physics.OdePlugin curr.W = dcur.W; Vector3 ax; - const int StopERP = 7; - const int StopCFM = 8; - int i = 0; int j = 0; if (axis.X == 0) @@ -943,10 +952,10 @@ namespace OpenSim.Region.Physics.OdePlugin d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, (int)StopCFM, 0f); - d.JointSetAMotorParam(Amotor, (int)StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); i++; - j = 256; // odeplugin.cs doesn't have all parameters so this moves to next axis set + j = 256; // move to next axis set } if (axis.Y == 0) @@ -960,8 +969,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); i++; j += 256; } @@ -977,8 +986,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); } } @@ -1186,24 +1195,10 @@ namespace OpenSim.Region.Physics.OdePlugin public void enableBodySoft() { - if (!childPrim) + if (!childPrim && !m_isSelected) { if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) { - if (m_targetSpace != _parent_scene.ActiveSpace) - { - m_targetSpace = _parent_scene.ActiveSpace; - - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom != IntPtr.Zero) - { - d.SpaceAdd(m_targetSpace, prm.prim_geom); - prm.m_targetSpace = m_targetSpace; - } - } - d.SpaceAdd(m_targetSpace, prim_geom); - } d.GeomEnable(prim_geom); foreach (OdePrim prm in childrenPrim) d.GeomEnable(prm.prim_geom); @@ -1211,6 +1206,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(Body); } } + m_disabled = false; resetCollisionAccounting(); // this sets m_disable to false } @@ -1221,19 +1217,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) { - if (m_targetSpace == _parent_scene.ActiveSpace) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.m_targetSpace != IntPtr.Zero && prm.prim_geom != IntPtr.Zero) - { - d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); - prm.m_targetSpace = IntPtr.Zero; - } - } - d.SpaceRemove(m_targetSpace, prim_geom); - m_targetSpace = IntPtr.Zero; - } d.GeomDisable(prim_geom); foreach (OdePrim prm in childrenPrim) d.GeomDisable(prm.prim_geom); @@ -1369,9 +1352,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetMass(Body, ref objdmass); _mass = objdmass.mass; - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - // disconnect from world gravity so we can apply buoyancy d.BodySetGravityMode(Body, false); @@ -1379,16 +1359,14 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableSteps(Body, body_autodisable_frames); // d.BodySetLinearDampingThreshold(Body, 0.01f); // d.BodySetAngularDampingThreshold(Body, 0.001f); - d.BodySetDamping(Body, .001f, .0002f); + d.BodySetDamping(Body, .002f, .002f); + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - - m_interpenetrationcount = 0; m_collisionscore = 0; - m_disabled = false; - if (m_targetSpace != _parent_scene.ActiveSpace) { if (m_targetSpace != IntPtr.Zero) @@ -1416,6 +1394,7 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + prm.m_collisionscore = 0; if (prm.m_targetSpace != _parent_scene.ActiveSpace) { @@ -1428,10 +1407,11 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_targetSpace = _parent_scene.ActiveSpace; d.SpaceAdd(m_targetSpace, prm.prim_geom); } - d.GeomEnable(prm.prim_geom); + + if (m_isSelected || m_disabled) + d.GeomDisable(prm.prim_geom); + prm.m_disabled = false; - prm.m_interpenetrationcount = 0; - prm.m_collisionscore = 0; _parent_scene.addActivePrim(prm); } } @@ -1442,8 +1422,12 @@ namespace OpenSim.Region.Physics.OdePlugin createAMotor(m_angularlock); } - d.GeomEnable(prim_geom); - m_disabled = false; + if (m_isSelected || m_disabled) + { + d.GeomDisable(prim_geom); + d.BodyDisable(Body); + } + _parent_scene.addActivePrim(this); } @@ -1484,12 +1468,16 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_collisionscore = 0; } } + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } d.BodyDestroy(Body); } Body = IntPtr.Zero; } _mass = primMass; - m_disabled = true; m_collisionscore = 0; } @@ -2115,49 +2103,72 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetTorque(Body, 0f, 0f, 0f); d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetAngularVel(Body, 0f, 0f, 0f); - } } private void changeSelectedStatus(bool newval) { + if (m_lastdoneSelected == newval) + return; + + m_lastdoneSelected = newval; + DoSelectedStatus(newval); + } + + private void CheckDelaySelect() + { + if (m_delaySelect) + { + DoSelectedStatus(m_isSelected); + } + } + + private void DoSelectedStatus(bool newval) + { m_isSelected = newval; Stop(); if (newval) { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + if (!childPrim && Body != IntPtr.Zero) + d.BodyDisable(Body); - if (prim_geom != IntPtr.Zero) + if (m_delaySelect) { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + d.GeomDisable(prm.prim_geom); + prm.m_delaySelect = false; + } + } + d.GeomDisable(prim_geom); + m_delaySelect = false; + } + else + { + m_delaySelect = true; } - - disableBodySoft(); } else { - m_collisionCategories = CollisionCategories.Geom; - - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags; - - if (m_collidesLand) - m_collisionFlags |= CollisionCategories.Land; - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; + if (!childPrim && Body != IntPtr.Zero && !m_disabled) + d.BodyEnable(Body); - if (prim_geom != IntPtr.Zero) + if (!childPrim) { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + foreach (OdePrim prm in childrenPrim) + { + if(!prm.m_disabled) + d.GeomEnable(prm.prim_geom); + prm.m_delaySelect = false; + } } + if(!m_disabled) + d.GeomEnable(prim_geom); - enableBodySoft(); + m_delaySelect = false; } resetCollisionAccounting(); @@ -2165,6 +2176,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changePosition(Vector3 newPos) { + CheckDelaySelect(); if (m_isphysical) { if (childPrim) // inertia is messed, must rebuild @@ -2207,6 +2219,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeOrientation(Quaternion newOri) { + CheckDelaySelect(); if (m_isphysical) { if (childPrim) // inertia is messed, must rebuild @@ -2258,6 +2271,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) { + CheckDelaySelect(); if (m_isphysical) { if (childPrim && m_building) // inertia is messed, must rebuild @@ -2342,6 +2356,8 @@ namespace OpenSim.Region.Physics.OdePlugin private void changePhysicsStatus(bool NewStatus) { + CheckDelaySelect(); + m_isphysical = NewStatus; if (!childPrim) @@ -2384,6 +2400,8 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeprimsizeshape() { + CheckDelaySelect(); + OdePrim parent = (OdePrim)_parent; bool chp = childPrim; @@ -2508,7 +2526,6 @@ namespace OpenSim.Region.Physics.OdePlugin } m_collisionscore = 0; - m_interpenetrationcount = 0; } } @@ -2528,7 +2545,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } m_collisionscore = 0; - m_interpenetrationcount = 0; } } @@ -2565,6 +2581,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { m_building = false; + CheckDelaySelect(); if (!childPrim) MakeBody(); } @@ -2575,18 +2592,26 @@ namespace OpenSim.Region.Physics.OdePlugin } } - private void changeVehicleType(int value) + public void changeSetVehicle(VehicleData vdata) { if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + m_vehicle.DoSetVehicle(vdata); + } + private void changeVehicleType(int value) + { + if (value == (int)Vehicle.TYPE_NONE) { - if (value != (int)Vehicle.TYPE_NONE) - { - m_vehicle = new ODEDynamics(this); - m_vehicle.ProcessTypeChange((Vehicle)value); - } + if (m_vehicle != null) + m_vehicle = null; } else + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + m_vehicle.ProcessTypeChange((Vehicle)value); + } } private void changeVehicleFloatParam(strVehicleFloatParam fp) @@ -2595,8 +2620,6 @@ namespace OpenSim.Region.Physics.OdePlugin return; m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); } private void changeVehicleVectorParam(strVehicleVectorParam vp) @@ -2604,8 +2627,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_vehicle == null) return; m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); } private void changeVehicleRotationParam(strVehicleQuatParam qp) @@ -2613,8 +2634,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_vehicle == null) return; m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); } private void changeVehicleFlags(strVehicleBoolParam bp) @@ -2622,8 +2641,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_vehicle == null) return; m_vehicle.ProcessVehicleFlags(bp.param, bp.value); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); } #endregion @@ -2849,41 +2866,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (Body != IntPtr.Zero) { - if (m_crossingfailures != 0 && m_crossingfailures < 5) - { - _position.X = Util.Clip(_position.X, 0.4f, _parent_scene.WorldExtents.X - 0.4f); - _position.Y = Util.Clip(_position.Y, 0.4f, _parent_scene.WorldExtents.Y - 0.4f); - _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); - - float tmp = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y); - if (_position.Z < tmp) - _position.Z = tmp + 0.2f; - - m_lastposition = _position; - m_lastorientation = _orientation; - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - m_lastVelocity = _velocity; - m_rotationalVelocity = _velocity; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - - m_crossingfailures = 0; // do this only once - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - enableBodySoft(); - base.RequestPhysicsterseUpdate(); - return; - } - - else if (m_crossingfailures != 0) - { - return; - } - Vector3 pv = Vector3.Zero; bool lastZeroFlag = _zeroFlag; @@ -2899,24 +2881,21 @@ namespace OpenSim.Region.Physics.OdePlugin // we can't let it keeping moving and having colisions // since it can be stucked between something like terrain and edge // so lets stop and disable it until something else kicks it - if (m_crossingfailures == 0) - { - _position.X = Util.Clip(lpos.X, -0.5f, _parent_scene.WorldExtents.X + 0.5f); - _position.Y = Util.Clip(lpos.Y, -0.5f, _parent_scene.WorldExtents.Y + 0.5f); - _position.Z = Util.Clip(lpos.Z, -100f, 50000f); + _position.X = Util.Clip(lpos.X, -0.2f, _parent_scene.WorldExtents.X + 0.2f); + _position.Y = Util.Clip(lpos.Y, -0.2f, _parent_scene.WorldExtents.Y + 0.2f); + _position.Z = Util.Clip(lpos.Z, -100f, 50000f); - m_lastposition = _position; - m_lastorientation = _orientation; + m_lastposition = _position; +// m_lastorientation = _orientation; - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - m_crossingfailures++; // do this only once - base.RequestPhysicsterseUpdate(); + d.BodySetLinearVel(Body, 0, 0, 0); // stop it +// d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + m_outbounds = true; + base.RequestPhysicsterseUpdate(); return; - } } if (lpos.Z < -100 || lpos.Z > 100000f) @@ -3159,6 +3138,7 @@ namespace OpenSim.Region.Physics.OdePlugin else ChildRemove(this, false); + m_vehicle = null; RemoveGeom(); m_targetSpace = IntPtr.Zero; if (m_eventsubscription > 0) @@ -3273,6 +3253,9 @@ namespace OpenSim.Region.Physics.OdePlugin changeVehicleRotationParam((strVehicleQuatParam) arg); break; + case changes.SetVehicle: + changeSetVehicle((VehicleData) arg); + break; case changes.Null: donullchange(); break; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index e62746e..2b6bc59 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -1,4 +1,3 @@ - /* * based on: * Ode.NET - .NET bindings for ODE diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 56f3786..6e4c373 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -137,6 +137,7 @@ namespace OpenSim.Region.Physics.OdePlugin VehicleVectorParam, VehicleRotationParam, VehicleFlags, + SetVehicle, Null //keep this last used do dim the methods array. does nothing but pulsing the prim } @@ -166,8 +167,8 @@ namespace OpenSim.Region.Physics.OdePlugin float frictionMovementMult = 0.3f; - float TerrainBounce = 0.3f; - float TerrainFriction = 0.3f; + float TerrainBounce = 0.1f; + float TerrainFriction = 0.1f; public float AvatarBounce = 0.3f; public float AvatarFriction = 0;// 0.9f * 0.5f; @@ -989,145 +990,62 @@ namespace OpenSim.Region.Physics.OdePlugin /// private void collision_optimized() { -// _perloopContact.Clear(); -// clear characts IsColliding until we do it some other way - lock (_characters) { - foreach (OdeCharacter chr in _characters) + try + { + foreach (OdeCharacter chr in _characters) { - // this are odd checks if they are needed something is wrong elsewhere - // keep for now - if (chr == null) - continue; - - if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) - continue; - - chr.IsColliding = false; - // chr.CollidingGround = false; not done here - chr.CollidingObj = false; + if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) + continue; + + chr.IsColliding = false; + // chr.CollidingGround = false; not done here + chr.CollidingObj = false; + // do colisions with static space + d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback); } } - - // now let ode do its job - // colide active things amoung them - - int st = Util.EnvironmentTickCount(); - int ta; - int ts; - try - { - d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); - } - catch (AccessViolationException) + catch (AccessViolationException) { - m_log.Warn("[PHYSICS]: Unable to Active space collide"); + m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); } - ta = Util.EnvironmentTickCountSubtract(st); - // then active things with static enviroment - try + + } + + // collide active prims with static enviroment + lock (_activeprims) + { + try { - d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback); + foreach (OdePrim prm in _activeprims) + { + if (d.BodyIsEnabled(prm.Body)) + d.SpaceCollide2(StaticSpace, prm.prim_geom, IntPtr.Zero, nearCallback); + } } - catch (AccessViolationException) + catch (AccessViolationException) { - m_log.Warn("[PHYSICS]: Unable to Active to static space collide"); + m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); } - ts = Util.EnvironmentTickCountSubtract(st); -// _perloopContact.Clear(); - } - - #endregion - - - public float GetTerrainHeightAtXY(float x, float y) - { - // assumes 1m size grid and constante size square regions - // region offset in mega position - - int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - - IntPtr heightFieldGeom = IntPtr.Zero; - - // get region map - if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) - return 0f; - - if (heightFieldGeom == IntPtr.Zero) - return 0f; - - if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) - return 0f; - - // TerrainHeightField for ODE as offset 1m - x += 1f - offsetX; - y += 1f - offsetY; - - // make position fit into array - if (x < 0) - x = 0; - if (y < 0) - y = 0; - - // integer indexs - int ix; - int iy; - // interpolators offset - float dx; - float dy; - - int regsize = (int)Constants.RegionSize + 2; // map size see setterrain - - // we still have square fixed size regions - // also flip x and y because of how map is done for ODE fliped axis - // so ix,iy,dx and dy are inter exchanged - if (x < regsize - 1) - { - iy = (int)x; - dy = x - (float)iy; - } - else // out world use external height - { - iy = regsize - 1; - dy = 0; } - if (y < regsize - 1) + + // finally colide active things amoung them + try { - ix = (int)y; - dx = y - (float)ix; + d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); } - else + catch (AccessViolationException) { - ix = regsize - 1; - dx = 0; + m_log.Warn("[PHYSICS]: Unable to collide in Active space"); } - float h0; - float h1; - float h2; - - iy *= regsize; - iy += ix; // all indexes have iy + ix +// _perloopContact.Clear(); + } - float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + #endregion - if ((dx + dy) <= 1.0f) - { - h0 = ((float)heights[iy]); // 0,0 vertice - h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 - h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 - } - else - { - h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice - h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 - h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 - } - return h0 + h1 + h2; - } /// /// Add actor to the list that should receive collision events in the simulate loop. @@ -1835,273 +1753,94 @@ namespace OpenSim.Region.Physics.OdePlugin get { return (false); } } - #region ODE Specific Terrain Fixes - public float[] ResizeTerrain512NearestNeighbour(float[] heightMap) + public float GetTerrainHeightAtXY(float x, float y) { - float[] returnarr = new float[262144]; - float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y]; + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future + // region offset in mega position - // Filling out the array into its multi-dimensional components - for (int y = 0; y < WorldExtents.Y; y++) - { - for (int x = 0; x < WorldExtents.X; x++) - { - resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x]; - } - } + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - // Resize using Nearest Neighbour - - // This particular way is quick but it only works on a multiple of the original - - // The idea behind this method can be described with the following diagrams - // second pass and third pass happen in the same loop really.. just separated - // them to show what this does. - - // First Pass - // ResultArr: - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - - // Second Pass - // ResultArr2: - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - - // Third pass fills in the blanks - // ResultArr2: - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - - // X,Y = . - // X+1,y = ^ - // X,Y+1 = * - // X+1,Y+1 = # - - // Filling in like this; - // .* - // ^# - // 1st . - // 2nd * - // 3rd ^ - // 4th # - // on single loop. - - float[,] resultarr2 = new float[512, 512]; - for (int y = 0; y < WorldExtents.Y; y++) - { - for (int x = 0; x < WorldExtents.X; x++) - { - resultarr2[y * 2, x * 2] = resultarr[y, x]; + IntPtr heightFieldGeom = IntPtr.Zero; - if (y < WorldExtents.Y) - { - resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x]; - } - if (x < WorldExtents.X) - { - resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x]; - } - if (x < WorldExtents.X && y < WorldExtents.Y) - { - resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x]; - } - } - } + // get region map + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return 0f; - //Flatten out the array - int i = 0; - for (int y = 0; y < 512; y++) - { - for (int x = 0; x < 512; x++) - { - if (resultarr2[y, x] <= 0) - returnarr[i] = 0.0000001f; - else - returnarr[i] = resultarr2[y, x]; + if (heightFieldGeom == IntPtr.Zero) + return 0f; - i++; - } - } + if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + return 0f; - return returnarr; - } + // TerrainHeightField for ODE as offset 1m + x += 1f - offsetX; + y += 1f - offsetY; - public float[] ResizeTerrain512Interpolation(float[] heightMap) - { - float[] returnarr = new float[262144]; - float[,] resultarr = new float[512,512]; + // make position fit into array + if (x < 0) + x = 0; + if (y < 0) + y = 0; + + // integer indexs + int ix; + int iy; + // interpolators offset + float dx; + float dy; + + int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples - // Filling out the array into its multi-dimensional components - for (int y = 0; y < 256; y++) + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) { - for (int x = 0; x < 256; x++) - { - resultarr[y, x] = heightMap[y * 256 + x]; - } + iy = (int)x; + dy = x - (float)iy; } - - // Resize using interpolation - - // This particular way is quick but it only works on a multiple of the original - - // The idea behind this method can be described with the following diagrams - // second pass and third pass happen in the same loop really.. just separated - // them to show what this does. - - // First Pass - // ResultArr: - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - - // Second Pass - // ResultArr2: - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - - // Third pass fills in the blanks - // ResultArr2: - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - - // X,Y = . - // X+1,y = ^ - // X,Y+1 = * - // X+1,Y+1 = # - - // Filling in like this; - // .* - // ^# - // 1st . - // 2nd * - // 3rd ^ - // 4th # - // on single loop. - - float[,] resultarr2 = new float[512,512]; - for (int y = 0; y < (int)Constants.RegionSize; y++) + else // out world use external height { - for (int x = 0; x < (int)Constants.RegionSize; x++) - { - resultarr2[y*2, x*2] = resultarr[y, x]; - - if (y < (int)Constants.RegionSize) - { - if (y + 1 < (int)Constants.RegionSize) - { - if (x + 1 < (int)Constants.RegionSize) - { - resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + - resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); - } - else - { - resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2); - } - } - else - { - resultarr2[(y*2) + 1, x*2] = resultarr[y, x]; - } - } - if (x < (int)Constants.RegionSize) - { - if (x + 1 < (int)Constants.RegionSize) - { - if (y + 1 < (int)Constants.RegionSize) - { - resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + - resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); - } - else - { - resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2); - } - } - else - { - resultarr2[y*2, (x*2) + 1] = resultarr[y, x]; - } - } - if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize) - { - if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize)) - { - resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + - resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); - } - else - { - resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; - } - } - } + iy = regsize - 1; + dy = 0; } - //Flatten out the array - int i = 0; - for (int y = 0; y < 512; y++) + if (y < regsize - 1) { - for (int x = 0; x < 512; x++) - { - if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) - { - m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0"); - resultarr2[y, x] = 0; - } - returnarr[i] = resultarr2[y, x]; - i++; - } + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsize - 1; + dx = 0; } - return returnarr; - } + float h0; + float h1; + float h2; - #endregion + iy *= regsize; + iy += ix; // all indexes have iy + ix + float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + + if ((dx + dy) <= 1.0f) + { + h0 = ((float)heights[iy]); // 0,0 vertice + h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + } + else + { + h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice + h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 + h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + } + + return h0 + h1 + h2; + } public override void SetTerrain(float[] heightMap) { if (m_worldOffset != Vector3.Zero && m_parentScene != null) @@ -2124,48 +1863,47 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetTerrain(float[] heightMap, Vector3 pOffset) { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future float[] _heightmap; - _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; uint heightmapWidth = Constants.RegionSize + 2; uint heightmapHeight = Constants.RegionSize + 2; - uint heightmapWidthSamples; + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; - uint heightmapHeightSamples; - - heightmapWidthSamples = (uint)Constants.RegionSize + 2; - heightmapHeightSamples = (uint)Constants.RegionSize + 2; + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; const float scale = 1.0f; const float offset = 0.0f; const float thickness = 10f; const int wrap = 0; - int regionsize = (int) Constants.RegionSize + 2; + uint regionsize = Constants.RegionSize; float hfmin = float.MaxValue; float hfmax = float.MinValue; float val; - int xx; - int yy; + uint xx; + uint yy; - int maxXXYY = regionsize - 3; + uint maxXXYY = regionsize - 1; // flipping map adding one margin all around so things don't fall in edges - int xt = 0; + uint xt = 0; xx = 0; - for (int x = 0; x < heightmapWidthSamples; x++) + for (uint x = 0; x < heightmapWidthSamples; x++) { if (x > 1 && xx < maxXXYY) xx++; yy = 0; - for (int y = 0; y < heightmapHeightSamples; y++) + for (uint y = 0; y < heightmapHeightSamples; y++) { if (y > 1 && y < maxXXYY) - yy += (int)Constants.RegionSize; + yy += regionsize; val = heightMap[yy + xx]; _heightmap[xt + y] = val; @@ -2176,8 +1914,7 @@ namespace OpenSim.Region.Physics.OdePlugin hfmax = val; } - - xt += regionsize; + xt += heightmapHeightSamples; } lock (OdeLock) { @@ -2230,11 +1967,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0); - IntPtr testGround = IntPtr.Zero; - if (RegionTerrain.TryGetValue(pOffset, out testGround)) - { - RegionTerrain.Remove(pOffset); - } RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); -- cgit v1.1 From a725b5e01ed92b54d0bddd321acbe4498492f67c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 27 Feb 2012 20:08:43 +0000 Subject: Let inworld meshs colide as such and not as basic prim all the time. Removed also LOD, and outerskin on colision meshs. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 6 +++--- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 9 +++++++-- 2 files changed, 10 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 86ab58d..915b88d 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -2421,7 +2421,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Don't need to re-enable body.. it's done in SetMesh try { - _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); + _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true); } catch { @@ -2676,7 +2676,7 @@ namespace OpenSim.Region.Physics.OdePlugin try { if (_parent_scene.needsMeshing(_pbs)) - mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true); } catch { @@ -2783,7 +2783,7 @@ namespace OpenSim.Region.Physics.OdePlugin meshlod = _parent_scene.MeshSculptphysicalLOD; try { - IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true); CreateGeom(m_targetSpace, mesh); } catch diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index f84918c..a57ebe9 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1772,7 +1772,7 @@ namespace OpenSim.Region.Physics.OdePlugin IMesh mesh = null; if (needsMeshing(pbs)) - mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical); + mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true); result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, localid); @@ -2514,7 +2514,7 @@ namespace OpenSim.Region.Physics.OdePlugin } // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim - if (!forceSimplePrimMeshing) + if (!forceSimplePrimMeshing && !pbs.SculptEntry) { if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 @@ -2537,6 +2537,9 @@ namespace OpenSim.Region.Physics.OdePlugin } } + if (forceSimplePrimMeshing) + return true; + if (pbs.ProfileHollow != 0) iPropertiesNotSupportedDefault++; @@ -2601,6 +2604,8 @@ namespace OpenSim.Region.Physics.OdePlugin } } + if (pbs.SculptEntry && meshSculptedPrim) + iPropertiesNotSupportedDefault++; if (iPropertiesNotSupportedDefault == 0) { -- cgit v1.1 From 9bad3b846f7f8a4be4b0c5b76ccf83c20ff0a7bf Mon Sep 17 00:00:00 2001 From: Melanie Date: Tue, 28 Feb 2012 10:37:46 +0100 Subject: Really dirty hack to make sims start. Causes errors but the ting runs. Remove when fixed right. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 915b88d..c0c33da 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -1810,7 +1810,11 @@ namespace OpenSim.Region.Physics.OdePlugin _size.X = 0.01f; _size.Y = 0.01f; _size.Z = 0.01f; - return false; + mesh.releaseSourceMeshData(); + // Temporarily hacking this to true which causes the object + // to get no physical representation, effectively making it + // phantom + return true; } -- cgit v1.1 From 1c7700db886291d3474cc30152abf5667bb467b6 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 28 Feb 2012 20:14:26 +0000 Subject: A few blind changes to go try to go around bad mesh little box replacement fail --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 117 +++++++++++--------------- 1 file changed, 50 insertions(+), 67 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index c0c33da..d63b9a4 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -1804,22 +1804,18 @@ namespace OpenSim.Region.Physics.OdePlugin mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid + mesh.releaseSourceMeshData(); // free up the original mesh data to save memory + if (vertexCount == 0 || indexCount == 0) { m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z); _size.X = 0.01f; _size.Y = 0.01f; _size.Z = 0.01f; - mesh.releaseSourceMeshData(); - // Temporarily hacking this to true which causes the object - // to get no physical representation, effectively making it - // phantom - return true; + return false; } - - // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid - mesh.releaseSourceMeshData(); // free up the original mesh data to save memory /* if (m_MeshToTriMeshMap.ContainsKey(mesh)) { @@ -1846,16 +1842,18 @@ namespace OpenSim.Region.Physics.OdePlugin SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); // } } - catch (AccessViolationException) + catch (Exception e) { - m_log.Error("[PHYSICS]: MESH LOCKED"); + m_log.ErrorFormat("[PHYSICS]: Create trimesh failed on prim {0} : {1}",Name,e.Message); if (_triMeshData != IntPtr.Zero) { d.GeomTriMeshDataDestroy(_triMeshData); _triMeshData = IntPtr.Zero; } - + _size.X = 0.01f; + _size.Y = 0.01f; + _size.Z = 0.01f; return false; } @@ -1878,6 +1876,9 @@ namespace OpenSim.Region.Physics.OdePlugin changeadd(timestep); } + if (m_taintremove) + return; + if (prim_geom != IntPtr.Zero) { if (!_position.ApproxEquals(m_taintposition, 0f)) @@ -2340,71 +2341,49 @@ namespace OpenSim.Region.Physics.OdePlugin gottrimesh = setMesh(_parent_scene, _mesh); } - if(!gottrimesh) // not a mesh + if (!gottrimesh) // not a mesh { - if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) // special profile?? + IntPtr geo = IntPtr.Zero; + + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.X == _size.Z) { - if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) // Equi-size + // its a sphere + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try { - if (((_size.X / 2f) > 0f)) // Has size - { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); - } - catch (AccessViolationException) - { - m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); - ode.dunlock(_parent_scene.world); - return; - } - } - else - { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); - } - catch (AccessViolationException) - { - m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); - ode.dunlock(_parent_scene.world); - return; - } - } + geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); } - else // not equi-size + catch (Exception e) { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); - } - catch (AccessViolationException) - { - m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); - ode.dunlock(_parent_scene.world); - return; - } + m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message); + geo = IntPtr.Zero; + ode.dunlock(_parent_scene.world); } } - - else // not special profile + else // make it a box { _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); } - catch (AccessViolationException) + catch (Exception e) { - m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); + m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message); + geo = IntPtr.Zero; ode.dunlock(_parent_scene.world); - return; } } + + if (geo == IntPtr.Zero) + { + m_taintremove = true; + _parent_scene.AddPhysicsActorTaint(this); + return; + } + + SetGeom(geo); } } @@ -2422,7 +2401,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (_parent_scene.needsMeshing(_pbs)) { - // Don't need to re-enable body.. it's done in SetMesh try { _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true); @@ -2431,9 +2409,9 @@ namespace OpenSim.Region.Physics.OdePlugin { //Don't continuously try to mesh prims when meshing has failed m_meshfailed = true; + _mesh = null; + m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z); } - // createmesh returns null when it's a shape that isn't a cube. - // m_log.Debug(m_localID); } } @@ -2685,12 +2663,12 @@ namespace OpenSim.Region.Physics.OdePlugin catch { m_meshfailed = true; + mesh = null; + m_log.WarnFormat("[PHYSICS]: changeSize CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z); } //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); CreateGeom(m_targetSpace, mesh); - - } else { @@ -2782,18 +2760,23 @@ namespace OpenSim.Region.Physics.OdePlugin { // Don't need to re-enable body.. it's done in SetMesh float meshlod = _parent_scene.meshSculptLOD; + IMesh mesh; if (IsPhysical) meshlod = _parent_scene.MeshSculptphysicalLOD; try { - IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true); - CreateGeom(m_targetSpace, mesh); + mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true); } catch { + mesh = null; m_meshfailed = true; + m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z); } + + CreateGeom(m_targetSpace, mesh); + // createmesh returns null when it doesn't mesh. } else -- cgit v1.1 From a8a7bb549d49b9b0cb3ff7478aa43f9e19931893 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 28 Feb 2012 20:24:03 +0000 Subject: made box a little larger --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index d63b9a4..0a4ebe4 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -1810,9 +1810,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (vertexCount == 0 || indexCount == 0) { m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z); - _size.X = 0.01f; - _size.Y = 0.01f; - _size.Z = 0.01f; + _size.X = 0.05f; + _size.Y = 0.05f; + _size.Z = 0.05f; return false; } @@ -1851,9 +1851,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomTriMeshDataDestroy(_triMeshData); _triMeshData = IntPtr.Zero; } - _size.X = 0.01f; - _size.Y = 0.01f; - _size.Z = 0.01f; + _size.X = 0.05f; + _size.Y = 0.05f; + _size.Z = 0.05f; return false; } -- cgit v1.1 From 5534cf8b57466396634f60695a3d8cfcce6009e2 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 Feb 2012 05:03:26 +0000 Subject: chODE: bad meshs get a a basic box or sphere geom with setted prim size. They will not colide if non-physical and Will collide with land if physical. assume UNTESTED --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 643 ++++++------------------ OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 8 +- 2 files changed, 148 insertions(+), 503 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 0a4ebe4..514074c 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -163,6 +163,8 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_isphysical; private bool m_isSelected; + private bool m_NoColide; // for now only for internal use for bad meshs + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively private bool m_throttleUpdates; @@ -253,7 +255,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_verticalAttractionEfficiency = 1.0f; // damped private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. - SerialControl m_taintserial = null; +// SerialControl m_taintserial = null; object m_taintvehicledata = null; public void DoSetVehicle() @@ -309,410 +311,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintvehicledata = vdata; _parent_scene.AddPhysicsActorTaint(this); } - - public override byte[] Serialize(bool PhysIsRunning) - { - SerialControl sc = new SerialControl(); - - lock (sc.alock) - { - if (PhysIsRunning) - { - m_taintserial = sc; - - if (!Monitor.Wait(sc.alock, 1000)) - { - m_log.Error("[chOde] prim data serialization timed out"); - m_taintserial = null; - return new byte[0]; - } - } - else - DoSerialize(sc); - } - - return sc.data; - } - - public void DoSerialize(SerialControl sc) - { - wstreamer st = new wstreamer(); - Vector3 vtmp; - - ushort version = 2; - if (!BitConverter.IsLittleEndian) - version |= 1; - st.Wushort(version); //version lower bit codes endian type for future use - - // compact booleans in a ushort - ushort flags = 0; - - if (m_isphysical) // this should be true for now - flags |= 1; - if (m_isSelected) - flags |= 2; - if (m_isVolumeDetect) - flags |= 4; - if (m_disabled) - flags |= 8; - if (m_collidesWater) - flags |= 16; - if (m_collidesLand) - flags |= 32; - if (m_usePID) - flags |= 64; - if (m_useAPID) - flags |= 128; - if (m_useHoverPID) - flags |= 256; - if (m_throttleUpdates) - flags |= 512; - - st.Wushort(flags); - - st.Wvector3(_size); - st.Wint(m_material); - st.Wfloat(m_density); - st.Wfloat(0); // future gravity mod V3 - st.Wfloat(0); // future friction V3 - st.Wfloat(0); // future bounce V3 - -// st.Wuint((uint)m_collisionCategories); -// st.Wuint((uint)m_collisionFlags); - - if (_parent == null) - { - st.Wvector3(_position); // ?? - st.Wquat(_orientation); - } - else // for childs save offsets - { - Quaternion to; - Quaternion ipo = Quaternion.Inverse(_parent.Orientation); - - if (m_isphysical && prim_geom != IntPtr.Zero) - { - d.Vector3 dvt; - d.GeomCopyPosition(prim_geom, out dvt); - - vtmp.X = dvt.X; - vtmp.Y = dvt.Y; - vtmp.Z = dvt.Z; - - d.Quaternion dqt; - d.GeomCopyQuaternion(prim_geom, out dqt); - - to.X = dqt.X; - to.Y = dqt.Y; - to.Z = dqt.Z; - to.W = dqt.W; // rotation in world - } - else - { - vtmp = _position; - to = _orientation; - } - - vtmp -= _parent.Position; // offset in world - vtmp *= ipo; // offset in local - st.Wvector3(vtmp); - - ipo *= to; // own rotation - st.Wquat(ipo); - } - - st.Wvector3(_velocity); - st.Wvector3(m_rotationalVelocity); - st.Wvector3(_acceleration); - st.Wvector3(m_rotateEnable); - - vtmp = Vector3.Zero; - for (int i = 0; i < m_forcelist.Count; i++) - { - - vtmp += (m_forcelist[i] * 100); - } - - st.Wvector3(vtmp); // force acc - - vtmp = Vector3.Zero; - for (int i = 0; i < m_angularforcelist.Count; i++) - { - vtmp += (m_angularforcelist[i] * 100); - } - - st.Wvector3(vtmp); // angular force acc - - st.Wvector3(m_PIDTarget); - st.Wfloat(m_PIDTau); - st.Wfloat(PID_D); - st.Wfloat(PID_G); - st.Wquat(m_APIDTarget); - st.Wfloat(m_APIDStrength); - st.Wfloat(m_APIDDamping); - st.Wfloat(m_APIDdamper); - - st.Wint((int)m_PIDHoverType); - st.Wfloat(m_PIDHoverHeight); - st.Wfloat(m_PIDHoverTau); - st.Wfloat(m_targetHoverHeight); - - st.Wfloat(m_groundHeight); - st.Wfloat(m_waterHeight); - - st.Wfloat(m_buoyancy); - - // this must be last since type none ends stream - if (m_type == Vehicle.TYPE_NONE) - st.Wint((int)Vehicle.TYPE_NONE); - else - { - st.Wint((int)m_type); - - st.Wquat(Quaternion.Identity); //m_referenceFrame - - st.Wint((int)m_flags); - - st.Wvector3(m_linearMotorDirection); - st.Wfloat( - (float)Math.Sqrt(m_lLinMotorDVel.LengthSquared() / m_linearMotorDirection.LengthSquared())); - - st.Wvector3(m_linearFrictionTimescale); - st.Wfloat(m_linearMotorDecayTimescale); - st.Wfloat(m_linearMotorTimescale); - st.Wvector3(new Vector3(0, 0, 0)); //m_linearMotorOffset); - - st.Wvector3(m_angularMotorDirection); - st.Wfloat((float)Math.Sqrt(m_angularMotorDVel.LengthSquared() / m_angularMotorDirection.LengthSquared())); - - st.Wvector3(m_angularFrictionTimescale); - st.Wfloat(m_angularMotorDecayTimescale); - st.Wfloat(m_angularMotorTimescale); - - st.Wfloat(0); //m_linearDeflectionEfficiency); - st.Wfloat(1000); //m_linearDeflectionTimescale); - - st.Wfloat(0); //m_angularDeflectionEfficiency); - st.Wfloat(120); //m_angularDeflectionTimescale); - - st.Wfloat(0); // m_bankingEfficiency); - st.Wfloat(0); //m_bankingMix); - st.Wfloat(1000); //m_bankingTimescale); - - st.Wfloat(m_VhoverHeight); - st.Wfloat(0.5f); //m_VhoverEfficiency); - st.Wfloat(m_VhoverTimescale); - - st.Wfloat(m_VehicleBuoyancy); - - st.Wfloat(m_verticalAttractionEfficiency); - st.Wfloat(m_verticalAttractionTimescale); - } - sc.data = st.close(); - m_taintserial = null; - Monitor.PulseAll(sc.alock); - } - - public bool DeSerialize(byte[] data) - { - rstreamer st = new rstreamer(data); - - int version =st.Rushort(); //version - - // merge booleans in a ushort - ushort flags = st.Rushort(); - if ((flags & 1) != 0) - m_isphysical = true; - if ((flags & 2) != 0) - m_taintselected = true; - if ((flags & 4) != 0) - m_isVolumeDetect = true; - if ((flags & 8) != 0) - m_taintdisable = true; - if ((flags & 16) != 0) - m_taintCollidesWater = true; - if ((flags & 32) != 0) - m_collidesLand = true; - if ((flags & 64) != 0) - m_usePID = true; - if ((flags & 128) != 0) - m_useAPID = true; - if ((flags & 256) != 0) - m_useHoverPID = true; - if ((flags & 512) != 0) - m_throttleUpdates = true; - - _size = st.Rvector3(); - m_taintsize = _size; - - m_material= st.Rint(); - m_density = st.Rfloat(); - st.Rfloat(); // future gravity mod V3 - st.Rfloat(); // future friction V3 - st.Rfloat(); // future bounce V3 - -// m_collisionCategories = (CollisionCategories)st.Ruint(); -// m_collisionFlags = (CollisionCategories) st.Ruint(); - - if (m_taintparent == null) - { - st.Rvector3(); // ignore old position sop/sog as to tell the new one - m_taintrot = st.Rquat(); // - _orientation = m_taintrot; - } - else - { - m_taintrot = _parent.Orientation; - m_taintposition = st.Rvector3(); // ?? - _position = m_taintposition; - - m_taintposition *= m_taintrot; - m_taintposition += _parent.Position; - - m_taintrot *= st.Rquat(); // - _orientation = m_taintrot; - } - - m_taintVelocity = st.Rvector3(); - m_rotationalVelocity = st.Rvector3(); - - _acceleration = st.Rvector3(); - m_rotateEnableRequest = st.Rvector3(); - m_rotateEnableUpdate = true; - - Vector3 vtmp; - - vtmp = st.Rvector3(); // forces acc - m_forcelist.Add(vtmp); - m_taintforce = true; - - vtmp = st.Rvector3(); // angular forces acc - m_angularforcelist.Add(vtmp); - m_taintaddangularforce = true; - - m_PIDTarget = st.Rvector3(); - m_PIDTau = st.Rfloat(); - PID_D = st.Rfloat(); - PID_G = st.Rfloat(); - - m_APIDTarget = st.Rquat(); - m_APIDStrength = st.Rfloat(); - m_APIDDamping = st.Rfloat(); - m_APIDdamper = st.Rfloat(); - - m_PIDHoverType = (PIDHoverType) st.Rint(); - m_PIDHoverHeight = st.Rfloat(); - m_PIDHoverTau = st.Rfloat(); - m_targetHoverHeight = st.Rfloat(); - - m_groundHeight = st.Rfloat(); - m_waterHeight = st.Rfloat(); - - m_buoyancy = st.Rfloat(); - - - // this must be last since type none ends stream - - m_type = (Vehicle) st.Rint(); - - if (m_type != Vehicle.TYPE_NONE) - { - float ftmp; - - st.Rquat(); //m_referenceFrame - - m_flags = (VehicleFlag) st.Rint(); - - m_linearMotorDirection = st.Rvector3(); - - ftmp = st.Rfloat(); - m_lLinMotorDVel = m_linearMotorDirection * ftmp; - - m_linearFrictionTimescale = st.Rvector3(); - m_linearMotorDecayTimescale = st.Rfloat(); - m_linearMotorTimescale = st.Rfloat(); - st.Rvector3(); //m_linearMotorOffset); - - m_angularMotorDirection = st.Rvector3(); - ftmp = st.Rfloat(); - m_angularMotorDVel = m_angularMotorDirection * ftmp; - - m_angularFrictionTimescale = st.Rvector3(); - m_angularMotorDecayTimescale = st.Rfloat(); - m_angularMotorTimescale = st.Rfloat(); - - st.Rfloat(); //m_linearDeflectionEfficiency); - st.Rfloat(); //m_linearDeflectionTimescale); - - st.Rfloat(); //m_angularDeflectionEfficiency); - st.Rfloat(); //m_angularDeflectionTimescale); - - st.Rfloat(); // m_bankingEfficiency); - st.Rfloat(); //m_bankingMix); - st.Rfloat(); //m_bankingTimescale); - - m_VhoverHeight = st.Rfloat(); - st.Rfloat(); //m_VhoverEfficiency); - m_VhoverTimescale = st.Rfloat(); - - m_VehicleBuoyancy = st.Rfloat(); - - m_verticalAttractionEfficiency = st.Rfloat(); - m_verticalAttractionTimescale = st.Rfloat(); - } - st.close(); - return true; - } - - public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, PhysicsActor parent, - PrimitiveBaseShape pbs, CollisionLocker dode, uint localid, byte[] sdata) - { - m_localID = localid; - ode = dode; - - if (parent == null) - { - m_taintparent = null; - - if (!pos.IsFinite()) - { - pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), - parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); - m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); - } - - _position = pos; - m_taintposition = pos; - } - else - m_taintparent = parent; - - body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - - prim_geom = IntPtr.Zero; - - _mesh = null; - m_meshfailed = false; - _pbs = pbs; - - _parent_scene = parent_scene; - m_targetSpace = (IntPtr)0; - - if(sdata != null && sdata.Length > 1) - DeSerialize(sdata); - - if (m_isphysical) - m_targetSpace = _parent_scene.space; - - _triMeshData = IntPtr.Zero; - - m_primName = primName; - m_taintserial = null; - m_taintadd = true; - _parent_scene.AddPhysicsActorTaint(this); - // don't do .add() here; old geoms get recycled with the same hash - } - public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) { @@ -776,8 +374,9 @@ namespace OpenSim.Region.Physics.OdePlugin } _triMeshData = IntPtr.Zero; + m_NoColide = false; - m_taintserial = null; +// m_taintserial = null; m_primName = primName; m_taintadd = true; _parent_scene.AddPhysicsActorTaint(this); @@ -814,7 +413,6 @@ namespace OpenSim.Region.Physics.OdePlugin { set { - //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); // This only makes the object not collidable if the object // is physical or the object is modified somehow *IN THE FUTURE* @@ -1077,7 +675,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override bool FloatOnWater { set @@ -1270,7 +867,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public void SetGeom(IntPtr geom) { if (prim_geom != IntPtr.Zero) @@ -1290,9 +886,23 @@ namespace OpenSim.Region.Physics.OdePlugin { _parent_scene.geom_name_map[prim_geom] = this.m_primName; _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + { + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + d.GeomDisable(prim_geom); + } + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } } if (childPrim) @@ -1351,11 +961,20 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.W = _orientation.W; d.BodySetQuaternion(Body, ref myrot); d.GeomSetBody(prim_geom, Body); + m_collisionCategories |= CollisionCategories.Body; m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); @@ -1723,11 +1342,19 @@ namespace OpenSim.Region.Physics.OdePlugin if (prim_geom != IntPtr.Zero) { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } } - d.BodyDestroy(Body); lock (childrenPrim) { @@ -1735,6 +1362,13 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdePrim prm in childrenPrim) { + if (prm.m_NoColide && prm.prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + d.GeomDisable(prm.prim_geom); + } + _parent_scene.remActivePrim(prm); prm.Body = IntPtr.Zero; } @@ -1752,8 +1386,18 @@ namespace OpenSim.Region.Physics.OdePlugin if (prim_geom != IntPtr.Zero) { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } @@ -1768,11 +1412,6 @@ namespace OpenSim.Region.Physics.OdePlugin public bool setMesh(OdeScene parent_scene, IMesh mesh) { - // This sleeper is there to moderate how long it takes between - // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object - - //Thread.Sleep(10); - //Kill Body so that mesh can re-make the geom if (IsPhysical && Body != IntPtr.Zero) { @@ -1790,14 +1429,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } -// do it on caller instead -/* - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } -*/ IntPtr vertices, indices; int vertexCount, indexCount; int vertexStride, triStride; @@ -1809,38 +1440,20 @@ namespace OpenSim.Region.Physics.OdePlugin if (vertexCount == 0 || indexCount == 0) { - m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z); - _size.X = 0.05f; - _size.Y = 0.05f; - _size.Z = 0.05f; + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); return false; } -/* - if (m_MeshToTriMeshMap.ContainsKey(mesh)) - { - _triMeshData = m_MeshToTriMeshMap[mesh]; - } - else -*/ - - + IntPtr geo = IntPtr.Zero; + try { _triMeshData = d.GeomTriMeshDataCreate(); - d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); d.GeomTriMeshDataPreprocess(_triMeshData); -// m_MeshToTriMeshMap[mesh] = _triMeshData; - } - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer - // { - // SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); - SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); - // } + _parent_scene.waitForSpaceUnlock(m_targetSpace); + + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); } catch (Exception e) { @@ -1851,21 +1464,11 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomTriMeshDataDestroy(_triMeshData); _triMeshData = IntPtr.Zero; } - _size.X = 0.05f; - _size.Y = 0.05f; - _size.Z = 0.05f; return false; } + SetGeom(geo); - // if (IsPhysical && Body == (IntPtr) 0) - // { - // Recreate the body - // m_interpenetrationcount = 0; - // m_collisionscore = 0; - - // enableBody(); - // } return true; } @@ -1943,18 +1546,21 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintvehicledata != null) DoSetVehicle(); - if (m_taintserial != null) - DoSerialize(m_taintserial); - /* obsolete if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) changeAngularLock(timestep); */ } + else { - m_log.Error("[PHYSICS]: The scene reused a disposed PhysActor! *waves finger*, Don't be evil. A couple of things can cause this. An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy! An improper buildup (creating the geom failed). Or, the Scene Reused a physics actor after disposing it.)"); + m_log.Error("[PHYSICS]: prim {0} at <{1},{2},{3}> as invalid geom"); + + // not sure this will not flame... + m_taintremove = true; + _parent_scene.AddPhysicsActorTaint(this); } + } /* obsolete @@ -2058,7 +1664,6 @@ namespace OpenSim.Region.Physics.OdePlugin } foreach (OdePrim prm in childrenPrim) { - prm.m_collisionCategories |= CollisionCategories.Body; prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); @@ -2067,9 +1672,17 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); continue; } - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + } d.Quaternion quat = new d.Quaternion(); quat.W = prm._orientation.W; @@ -2098,20 +1711,28 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body"); } - prm.m_interpenetrationcount = 0; prm.m_collisionscore = 0; prm.m_disabled = false; prm.Body = Body; - _parent_scene.addActivePrim(prm); + + _parent_scene.addActivePrim(prm); } + m_collisionCategories |= CollisionCategories.Body; m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } d.Quaternion quat2 = new d.Quaternion(); quat2.W = _orientation.W; @@ -2134,19 +1755,18 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - m_interpenetrationcount = 0; m_collisionscore = 0; m_disabled = false; d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene); + _parent_scene.addActivePrim(this); } } } } - } private void ChildSetGeom(OdePrim odePrim) @@ -2258,6 +1878,8 @@ namespace OpenSim.Region.Physics.OdePlugin { d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_NoColide) + d.GeomDisable(prim_geom); } if (m_isphysical) @@ -2275,22 +1897,35 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - m_collisionCategories = CollisionCategories.Geom; - - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags = m_default_collisionFlags; + m_collisionFlags = m_default_collisionFlags; - if (m_collidesLand) - m_collisionFlags |= CollisionCategories.Land; - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; + if (m_collidesLand) + m_collisionFlags |= CollisionCategories.Land; + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; if (prim_geom != IntPtr.Zero) { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } } if (Body != IntPtr.Zero) { @@ -2330,19 +1965,29 @@ namespace OpenSim.Region.Physics.OdePlugin { bool gottrimesh = false; + m_NoColide = false; // assume all will go well + if (_triMeshData != IntPtr.Zero) { d.GeomTriMeshDataDestroy(_triMeshData); _triMeshData = IntPtr.Zero; } - if (_mesh != null) // Special - make mesh + if (_mesh != null) { gottrimesh = setMesh(_parent_scene, _mesh); + if (!gottrimesh) + { + // getting a mesh failed, + // lets go on having a basic box or sphere, with prim size but not coliding + // physical colides with land, non with nothing + + m_NoColide = true; + } } - if (!gottrimesh) // not a mesh - { + if (!gottrimesh) + { // we will have a basic box or sphere IntPtr geo = IntPtr.Zero; if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 @@ -2376,14 +2021,17 @@ namespace OpenSim.Region.Physics.OdePlugin } } - if (geo == IntPtr.Zero) + if (geo == IntPtr.Zero) // if this happens it must be fixed { + // if it does lets stop what we can + // not sure this will not flame... + m_taintremove = true; _parent_scene.AddPhysicsActorTaint(this); return; } - SetGeom(geo); + SetGeom(geo); // this processes the m_NoColide } } @@ -2415,7 +2063,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - lock (_parent_scene.OdeLock) { CreateGeom(m_targetSpace, _mesh); @@ -2518,8 +2165,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintposition = _position; } - - public void rotate(float timestep) { d.Quaternion myrot = new d.Quaternion(); diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index a57ebe9..0266ef0 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1735,7 +1735,7 @@ namespace OpenSim.Region.Physics.OdePlugin return newPrim; } - +/* private PhysicsActor AddPrim(String name, Vector3 position, PhysicsActor parent, PrimitiveBaseShape pbs, uint localid, byte[] sdata) { @@ -1751,7 +1751,7 @@ namespace OpenSim.Region.Physics.OdePlugin return newPrim; } - +*/ public void addActivePrim(OdePrim activatePrim) { @@ -1778,7 +1778,7 @@ namespace OpenSim.Region.Physics.OdePlugin return result; } - +/* public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, uint localid, byte[] sdata) { @@ -1789,7 +1789,7 @@ namespace OpenSim.Region.Physics.OdePlugin return result; } - +*/ public override float TimeDilation { get { return m_timeDilation; } -- cgit v1.1 From a0d75be56be66e7e7dd04c8ae7760596d135af99 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 Feb 2012 05:14:29 +0000 Subject: fix --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 514074c..3ed3b5a 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -893,10 +893,12 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical) { d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - d.GeomDisable(prim_geom); } else + { d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } } else { -- cgit v1.1 From a8f1cd7e8643fc6f4f4f12b1af3e52b9830f5a5a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 Feb 2012 19:32:23 +0000 Subject: update ubitODE to current working state --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 189 ++++++++++++++--------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 123 +++++++++------ 2 files changed, 199 insertions(+), 113 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 0ccdbc0..17f38e8 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -155,6 +155,8 @@ namespace OpenSim.Region.Physics.OdePlugin public float m_collisionscore; int m_colliderfilter = 0; + public IntPtr collide_geom; // for objects: geom if single prim space it linkset + private float m_density = 10.000006836f; // Aluminum g/cm3; public bool _zeroFlag; @@ -453,8 +455,6 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - // Averate previous velocity with the new one so - // client object interpolation works a 'little' better if (_zeroFlag) return Vector3.Zero; return _velocity; @@ -833,6 +833,7 @@ namespace OpenSim.Region.Physics.OdePlugin body_autodisable_frames = parent_scene.bodyFramesAutoDisable; prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; Body = IntPtr.Zero; if (!size.IsFinite()) @@ -1367,7 +1368,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); m_collisionscore = 0; - if (m_targetSpace != _parent_scene.ActiveSpace) +// if (m_targetSpace != _parent_scene.ActiveSpace) { if (m_targetSpace != IntPtr.Zero) { @@ -1376,9 +1377,26 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceRemove(m_targetSpace, prim_geom); } +// m_targetSpace = _parent_scene.ActiveSpace; +// d.SpaceAdd(m_targetSpace, prim_geom); + } + + + if (childrenPrim.Count == 0) + { + collide_geom = prim_geom; m_targetSpace = _parent_scene.ActiveSpace; d.SpaceAdd(m_targetSpace, prim_geom); } + else + { + m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); + d.HashSpaceSetLevels(m_targetSpace, -2, 8); + d.SpaceSetSublevel(m_targetSpace, 3); + d.SpaceSetCleanup(m_targetSpace, false); + d.SpaceAdd(m_targetSpace, prim_geom); + collide_geom = m_targetSpace; + } lock (childrenPrim) { @@ -1396,15 +1414,15 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); prm.m_collisionscore = 0; - if (prm.m_targetSpace != _parent_scene.ActiveSpace) + if (prm.m_targetSpace != m_targetSpace) { if (prm.m_targetSpace != IntPtr.Zero) { - _parent_scene.waitForSpaceUnlock(m_targetSpace); + _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); } - prm.m_targetSpace = _parent_scene.ActiveSpace; + prm.m_targetSpace = m_targetSpace; d.SpaceAdd(m_targetSpace, prm.prim_geom); } @@ -1427,8 +1445,14 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomDisable(prim_geom); d.BodyDisable(Body); } + else + { + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } _parent_scene.addActivePrim(this); + _parent_scene.addActiveGroups(this); } private void DestroyBody() @@ -1473,6 +1497,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.JointDestroy(Amotor); Amotor = IntPtr.Zero; } + _parent_scene.remActiveGroup(this); d.BodyDestroy(Body); } Body = IntPtr.Zero; @@ -2390,8 +2415,8 @@ namespace OpenSim.Region.Physics.OdePlugin else */ DestroyBody(); - Stop(); } + Stop(); } } @@ -2576,7 +2601,8 @@ namespace OpenSim.Region.Physics.OdePlugin if ((bool)newbuilding) { m_building = true; - DestroyBody(); + if (!childPrim) + DestroyBody(); } else { @@ -2648,12 +2674,95 @@ namespace OpenSim.Region.Physics.OdePlugin public void Move() { if (!childPrim && m_isphysical && Body != IntPtr.Zero && - !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building) // KF: Only move root prims. + !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) + // !m_disabled && !m_isSelected && !m_building && !m_outbounds) { - // if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 +// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 float timestep = _parent_scene.ODE_STEPSIZE; + // check outside region + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; + + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if(m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } + + float fx = 0; float fy = 0; float fz = 0; @@ -2862,7 +2971,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void UpdatePositionAndVelocity(float simulatedtime) { // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null && !m_disabled && !m_building) + if (_parent == null && !m_disabled && !m_building && !m_outbounds) { if (Body != IntPtr.Zero) { @@ -2872,64 +2981,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.Vector3 lpos; d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - // we need to use root position since that's all the rest of scene uses - if (lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X - || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y - ) - { - // we are outside current region - // we can't let it keeping moving and having colisions - // since it can be stucked between something like terrain and edge - // so lets stop and disable it until something else kicks it - - _position.X = Util.Clip(lpos.X, -0.2f, _parent_scene.WorldExtents.X + 0.2f); - _position.Y = Util.Clip(lpos.Y, -0.2f, _parent_scene.WorldExtents.Y + 0.2f); - _position.Z = Util.Clip(lpos.Z, -100f, 50000f); - - m_lastposition = _position; -// m_lastorientation = _orientation; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it -// d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - m_outbounds = true; - base.RequestPhysicsterseUpdate(); - return; - } - - if (lpos.Z < -100 || lpos.Z > 100000f) - { - lpos.Z = Util.Clip(lpos.Z, -100f, 50000f); - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - - return; - } d.Quaternion ori; d.GeomCopyQuaternion(prim_geom, out ori); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 6e4c373..ccb4c23 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -224,6 +224,7 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly HashSet _characters = new HashSet(); private readonly HashSet _prims = new HashSet(); private readonly HashSet _activeprims = new HashSet(); + private readonly HashSet _activegroups = new HashSet(); public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); @@ -1013,15 +1014,24 @@ namespace OpenSim.Region.Physics.OdePlugin } - // collide active prims with static enviroment lock (_activeprims) { + foreach (OdePrim aprim in _activeprims) + { + aprim.CollisionScore = 0; + aprim.IsColliding = false; + } + } + + // collide active prims with static enviroment + lock (_activegroups) + { try { - foreach (OdePrim prm in _activeprims) + foreach (OdePrim prm in _activegroups) { - if (d.BodyIsEnabled(prm.Body)) - d.SpaceCollide2(StaticSpace, prm.prim_geom, IntPtr.Zero, nearCallback); + if (d.BodyIsEnabled(prm.Body) && !prm.m_outbounds) + d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); } } catch (AccessViolationException) @@ -1029,7 +1039,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); } } - // finally colide active things amoung them try { @@ -1039,7 +1048,6 @@ namespace OpenSim.Region.Physics.OdePlugin { m_log.Warn("[PHYSICS]: Unable to collide in Active space"); } - // _perloopContact.Clear(); } @@ -1148,13 +1156,20 @@ namespace OpenSim.Region.Physics.OdePlugin public void addActivePrim(OdePrim activatePrim) { - // adds active prim.. (ones that should be iterated over in collisions_optimized + // adds active prim.. lock (_activeprims) { if (!_activeprims.Contains(activatePrim)) _activeprims.Add(activatePrim); - //else - // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); + } + } + + public void addActiveGroups(OdePrim activatePrim) + { + lock (_activegroups) + { + if (!_activegroups.Contains(activatePrim)) + _activegroups.Add(activatePrim); } } @@ -1186,6 +1201,13 @@ namespace OpenSim.Region.Physics.OdePlugin _activeprims.Remove(deactivatePrim); } } + public void remActiveGroup(OdePrim deactivatePrim) + { + lock (_activegroups) + { + _activegroups.Remove(deactivatePrim); + } + } public override void RemovePrim(PhysicsActor prim) { @@ -1258,6 +1280,11 @@ namespace OpenSim.Region.Physics.OdePlugin { waitForSpaceUnlock(currentspace); d.SpaceRemove(currentspace, geom); + + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) + { + d.SpaceDestroy(currentspace); + } } else { @@ -1274,6 +1301,12 @@ namespace OpenSim.Region.Physics.OdePlugin { waitForSpaceUnlock(currentspace); d.SpaceRemove(currentspace, geom); + + if (d.SpaceGetSublevel(currentspace) > 1 && d.SpaceGetNumGeoms(currentspace) == 0) + { + d.SpaceDestroy(currentspace); + } + } } } @@ -1577,42 +1610,14 @@ namespace OpenSim.Region.Physics.OdePlugin statchanges += Util.EnvironmentTickCountSubtract(statstart); - // Move characters - lock (_characters) - { - List defects = new List(); - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - actor.Move(ODE_STEPSIZE, defects); - } - if (defects.Count != 0) - { - foreach (OdeCharacter defect in defects) - { - RemoveCharacter(defect); - } - } - } - statchmove += Util.EnvironmentTickCountSubtract(statstart); - - // Move other active objects - lock (_activeprims) - { - foreach (OdePrim aprim in _activeprims) - { - aprim.CollisionScore = 0; - aprim.IsColliding = false; - aprim.Move(); - } - } - statactmove += Util.EnvironmentTickCountSubtract(statstart); //if ((framecount % m_randomizeWater) == 0) // randomizeWater(waterlevel); m_rayCastManager.ProcessQueuedRequests(); + + statray += Util.EnvironmentTickCountSubtract(statstart); collision_optimized(); statcol += Util.EnvironmentTickCountSubtract(statstart); @@ -1642,8 +1647,35 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldQuickStep(world, ODE_STEPSIZE); statstep += Util.EnvironmentTickCountSubtract(statstart); - d.JointGroupEmpty(contactgroup); - totjcontact += m_global_contactcount; + + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(ODE_STEPSIZE, defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + } + } + statchmove += Util.EnvironmentTickCountSubtract(statstart); + + // Move other active objects + lock (_activegroups) + { + foreach (OdePrim aprim in _activegroups) + { + aprim.Move(); + } + } + //ode.dunlock(world); } catch (Exception e) @@ -1652,6 +1684,9 @@ namespace OpenSim.Region.Physics.OdePlugin // ode.dunlock(world); } + d.JointGroupEmpty(contactgroup); + totjcontact += m_global_contactcount; + step_time -= ODE_STEPSIZE; nodeframes++; } @@ -1686,10 +1721,10 @@ namespace OpenSim.Region.Physics.OdePlugin } statmovchar = Util.EnvironmentTickCountSubtract(statstart); - lock (_activeprims) + lock (_activegroups) { { - foreach (OdePrim actor in _activeprims) + foreach (OdePrim actor in _activegroups) { if (actor.IsPhysical) { @@ -1966,7 +2001,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0); + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); -- cgit v1.1 From ee4d3bfad2c9660637c24195a68d3f4484fed0b7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 Feb 2012 19:36:16 +0000 Subject: ubitODE bug fix --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index ccb4c23..eb5c687 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1302,7 +1302,7 @@ namespace OpenSim.Region.Physics.OdePlugin waitForSpaceUnlock(currentspace); d.SpaceRemove(currentspace, geom); - if (d.SpaceGetSublevel(currentspace) > 1 && d.SpaceGetNumGeoms(currentspace) == 0) + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) { d.SpaceDestroy(currentspace); } -- cgit v1.1 From f42fd7fb773cc317f36e747579e9f8e28cfe2049 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 2 Mar 2012 15:05:30 +0000 Subject: update ubitODE --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 60 ++++++++++++++++------ 1 file changed, 45 insertions(+), 15 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index c9d0909..4f82c24 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -38,7 +38,7 @@ * settings use. */ -// Ubit 2012 +// Extensive change Ubit 2012 using System; using System.Collections.Generic; @@ -614,6 +614,7 @@ namespace OpenSim.Region.Physics.OdePlugin return vec; } + private const float pi = (float)Math.PI; private const float halfpi = 0.5f * (float)Math.PI; public static Vector3 ubitRot2Euler(Quaternion rot) @@ -884,35 +885,64 @@ namespace OpenSim.Region.Physics.OdePlugin float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; - if (Math.Abs(roll) > 0.01) // roll + if (roll > halfpi) + roll = pi - roll; + else if (roll < -halfpi) + roll = -pi - roll; + + float effroll = pitch / halfpi; + effroll *= effroll; + effroll = 1 - effroll; + effroll *= roll; + + if (Math.Abs(effroll) > 0.01) // roll { - torque.X -= -roll * ftmp + curLocalAngVel.X * ftmp2; + torque.X -= -effroll * ftmp + curLocalAngVel.X * ftmp2; } - if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch + if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0) { - torque.Y -= -pitch * ftmp + curLocalAngVel.Y * ftmp2; + float effpitch = roll / halfpi; + effpitch *= effpitch; + effpitch = 1 - effpitch; + effpitch *= pitch; + + if (Math.Abs(effpitch) > 0.01) // pitch + { + torque.Y -= -effpitch * ftmp + curLocalAngVel.Y * ftmp2; + } } - if (m_bankingEfficiency != 0 && Math.Abs(roll) > 0.01) + if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01) { - float broll = roll * m_bankingEfficiency; ; + + float broll = effroll; +/* + if (broll > halfpi) + broll = pi - broll; + else if (broll < -halfpi) + broll = -pi - broll; +*/ + broll *= m_bankingEfficiency; if (m_bankingMix != 0) { float vfact = Math.Abs(curLocalVel.X) / 10.0f; if (vfact > 1.0f) vfact = 1.0f; + if (curLocalVel.X >= 0) - broll *= ((1 - m_bankingMix) + vfact); + broll *= (1 + (vfact - 1) * m_bankingMix); else - broll *= -((1 - m_bankingMix) + vfact); + broll *= -(1 + (vfact - 1) * m_bankingMix); } - broll = (broll - curLocalAngVel.Z) / m_bankingTimescale; - // torque.Z += broll; - // make z rot be in world Z not local as seems to be in sl - tmpV.X = 0; - tmpV.Y = 0; - tmpV.Z = broll; + + broll = broll / m_bankingTimescale; + + ftmp = -Math.Abs(m_bankingEfficiency) / m_bankingTimescale; + + tmpV.X = ftmp * curAngVel.X; + tmpV.Y = ftmp * curAngVel.Y; + tmpV.Z = broll + ftmp * curAngVel.Z; tmpV *= irotq; torque.X += tmpV.X; -- cgit v1.1 From 458f295fdbc16a9861e1f6ee19fbfb54230a138b Mon Sep 17 00:00:00 2001 From: Melanie Date: Fri, 2 Mar 2012 22:19:28 +0100 Subject: This still causes terrain artefacts Revert " fix the last fix. Regions are square but... Also remove the 0.5 offset in map position. It was apparently needed to fix we having nsamples = size and not size + 1." This reverts commit 15bc539bd49e7a09c1ec6e539871cde5eee6032e. --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index a57ebe9..ccdd32e 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -3495,7 +3495,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Output x = 0 1 2 3 ..... 255 256 257 258 total out float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255> if (val < minele) val = minele; - _heightmap[x * (heightmapHeightSamples) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> + _heightmap[x * (heightmapWidthSamples) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> hfmin = (val < hfmin) ? val : hfmin; hfmax = (val > hfmax) ? val : hfmax; } @@ -3545,8 +3545,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0); - // having nsamples = size + 1 center is actually at size/2 - d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)), (pOffset.Y + (regionsize * 0.5f)), 0); IntPtr testGround = IntPtr.Zero; if (RegionTerrain.TryGetValue(pOffset, out testGround)) { -- cgit v1.1 From bfcba0a417f876a09eae7612f08919304f7513e3 Mon Sep 17 00:00:00 2001 From: Melanie Date: Fri, 2 Mar 2012 22:20:02 +0100 Subject: This still causes terrain artefacts Revert " fix my bug on ChODE terrain heightmap build" This reverts commit aa77d1d486f11da7dc841190f1ca85e085d0d648. --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index ccdd32e..7a1e671 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -3457,8 +3457,8 @@ namespace OpenSim.Region.Physics.OdePlugin int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra int heightmapHeight = regionsize + 2; - int heightmapWidthSamples = (int)regionsize + 3; // Sample file size, 258 x 258 samples - int heightmapHeightSamples = (int)regionsize + 3; + int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples + int heightmapHeightSamples = (int)regionsize + 2; // Array of height samples for ODE float[] _heightmap; @@ -3495,7 +3495,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Output x = 0 1 2 3 ..... 255 256 257 258 total out float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255> if (val < minele) val = minele; - _heightmap[x * (heightmapWidthSamples) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> + _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257> hfmin = (val < hfmin) ? val : hfmin; hfmax = (val > hfmax) ? val : hfmax; } -- cgit v1.1 From 7c931037bd9d43b7de52534870ad15a645fe67d6 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 4 Mar 2012 01:40:12 +0000 Subject: update UbitODE --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 11 ++++ OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 12 ++++ OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 68 ++++++++++------------ 3 files changed, 55 insertions(+), 36 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 4f82c24..0fabb56 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -120,6 +120,16 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_lmEfect = 0; // current linear motor eficiency private float m_amEfect = 0; // current angular motor eficiency + public bool EngineActive + { + get + { + if (m_lmEfect > 0.01) + return true; + return false; + } + } + public ODEDynamics(OdePrim rootp) { @@ -152,6 +162,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorTimescale = vd.m_linearMotorTimescale; if (m_linearMotorTimescale < timestep) m_linearMotorTimescale = timestep; + m_linearMotorOffset = vd.m_linearMotorOffset; //Angular properties diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 17f38e8..e5fa1d7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -222,6 +222,18 @@ namespace OpenSim.Region.Physics.OdePlugin { get { + if (m_isphysical) + { + ODEDynamics veh; + if (_parent != null) + veh = ((OdePrim)_parent).m_vehicle; + else + veh = m_vehicle; + + if (veh != null) + if (veh.Type != Vehicle.TYPE_NONE && veh.EngineActive) + return new ContactData(0, 0); + } return primContactData; } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index eb5c687..129db5d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -155,7 +155,7 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly ILog m_log; // private Dictionary m_storedCollisions = new Dictionary(); - private int threadid = 0; +// private int threadid = 0; private Random fluidRandomizer = new Random(Environment.TickCount); const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; @@ -168,7 +168,7 @@ namespace OpenSim.Region.Physics.OdePlugin float frictionMovementMult = 0.3f; float TerrainBounce = 0.1f; - float TerrainFriction = 0.1f; + float TerrainFriction = 0.3f; public float AvatarBounce = 0.3f; public float AvatarFriction = 0;// 0.9f * 0.5f; @@ -189,8 +189,8 @@ namespace OpenSim.Region.Physics.OdePlugin internal IntPtr WaterGeom; - public float avPIDD = 3200f; // make it visible - public float avPIDP = 1400f; // make it visible + public float avPIDD = 2200f; // make it visible + public float avPIDP = 900f; // make it visible private float avCapRadius = 0.37f; private float avDensity = 3f; private float avMovementDivisorWalk = 1.3f; @@ -202,7 +202,7 @@ namespace OpenSim.Region.Physics.OdePlugin public bool forceSimplePrimMeshing = false; public float meshSculptLOD = 32; - public float MeshSculptphysicalLOD = 16; + public float MeshSculptphysicalLOD = 32; public float geomDefaultDensity = 10.000006836f; @@ -212,12 +212,11 @@ namespace OpenSim.Region.Physics.OdePlugin public float bodyPIDD = 35f; public float bodyPIDG = 25; - public int geomCrossingFailuresBeforeOutofbounds = 6; +// public int geomCrossingFailuresBeforeOutofbounds = 6; public int bodyFramesAutoDisable = 20; private float[] _watermap; - private bool m_filterCollisions = true; private d.NearCallback nearCallback; @@ -388,9 +387,6 @@ namespace OpenSim.Region.Physics.OdePlugin // Defaults - avPIDD = 2200.0f; - avPIDP = 900.0f; - int contactsPerCollision = 80; if (m_config != null) @@ -398,57 +394,56 @@ namespace OpenSim.Region.Physics.OdePlugin IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; if (physicsconfig != null) { - gravityx = physicsconfig.GetFloat("world_gravityx", 0f); - gravityy = physicsconfig.GetFloat("world_gravityy", 0f); - gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f); + gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); + gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); + gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); - metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); - ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f); - m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); + ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); + m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations); avDensity = physicsconfig.GetFloat("av_density", avDensity); - avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); - avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); - avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); + avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius); - contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); - geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); +// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); - geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); - bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); + bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); - bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f); - bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f); + bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD); + bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG); forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); - meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); - meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f); - MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f); - m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false); + meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim); + meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD); + MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); if (Environment.OSVersion.Platform == PlatformID.Unix) { - avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f); - avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f); + avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", avPIDP); } else { - avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); - avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); + avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP); } physics_logging = physicsconfig.GetBoolean("physics_logging", false); physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); - minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); - maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); + minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); } } @@ -1941,13 +1936,14 @@ namespace OpenSim.Region.Physics.OdePlugin yy += regionsize; val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode _heightmap[xt + y] = val; if (hfmin > val) hfmin = val; if (hfmax < val) hfmax = val; - } xt += heightmapHeightSamples; } -- cgit v1.1 From 01fcd400d7651be5f4aae3547a0489a7ccc5d7f7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 4 Mar 2012 04:26:05 +0000 Subject: update UbitOde --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 5 +- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 12 +++-- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 19 ++++--- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 39 +++++++------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 59 ++++++++++------------ 5 files changed, 69 insertions(+), 65 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index f525e9e..25aa4dc 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -158,9 +158,10 @@ namespace OpenSim.Region.Physics.Manager public virtual bool Building { get; set; } - public virtual ContactData ContactData + public virtual void getContactData(ref ContactData cdata) { - get { return new ContactData(0, 0); } + cdata.mu = 0; + cdata.bounce = 0; } public abstract bool Stopped { get; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 793e281..94cadb2 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -136,7 +136,8 @@ namespace OpenSim.Region.Physics.OdePlugin public UUID m_uuid; public bool bad = false; - public ContactData AvatarContactData = new ContactData(10f, 0.3f); + float mu; + float bounce; public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) { @@ -168,8 +169,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_density = density; m_mass = 80f; // sure we have a default - AvatarContactData.mu = parent_scene.AvatarFriction; - AvatarContactData.bounce = parent_scene.AvatarBounce; + mu = parent_scene.AvatarFriction; + bounce = parent_scene.AvatarBounce; walkDivisor = walk_divisor; runDivisor = rundivisor; @@ -190,9 +191,10 @@ namespace OpenSim.Region.Physics.OdePlugin set { return; } } - public override ContactData ContactData + public override void getContactData(ref ContactData cdata) { - get { return AvatarContactData; } + cdata.mu = mu; + cdata.bounce = bounce; } public override bool Building { get; set; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 0fabb56..d0b4546 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -119,25 +119,22 @@ namespace OpenSim.Region.Physics.OdePlugin // auxiliar private float m_lmEfect = 0; // current linear motor eficiency private float m_amEfect = 0; // current angular motor eficiency + private float m_ffactor = 1.0f; - public bool EngineActive + public float FrictionFactor { get { - if (m_lmEfect > 0.01) - return true; - return false; + return m_ffactor; } } - public ODEDynamics(OdePrim rootp) { rootPrim = rootp; _pParentScene = rootPrim._parent_scene; } - public void DoSetVehicle(VehicleData vd) { @@ -212,6 +209,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_lmEfect = 0; m_amEfect = 0; + m_ffactor = 1.0f; } internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) @@ -329,6 +327,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 30.0f) m_linearMotorDirection *= (30.0f / len); m_lmEfect = 1.0f; // turn it on + m_ffactor = 0.01f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) d.BodyEnable(rootPrim.Body); @@ -379,6 +378,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 30.0f) m_linearMotorDirection *= (30.0f / len); m_lmEfect = 1.0f; // turn it on + m_ffactor = 0.01f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) d.BodyEnable(rootPrim.Body); @@ -425,6 +425,7 @@ namespace OpenSim.Region.Physics.OdePlugin float invtimestep = _pParentScene.ODE_STEPSIZE; m_lmEfect = 0; m_amEfect = 0; + m_ffactor = 1f; m_linearMotorDirection = Vector3.Zero; m_angularMotorDirection = Vector3.Zero; @@ -602,6 +603,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_lmEfect = 0; m_amEfect = 0; + m_ffactor = 1f; } public static Vector3 Xrot(Quaternion rot) @@ -752,9 +754,14 @@ namespace OpenSim.Region.Physics.OdePlugin force.Z += tmpV.Z; } m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale); + + m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); } else + { m_lmEfect = 0; + m_ffactor = 1f; + } // friction if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index e5fa1d7..db07565 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -187,7 +187,8 @@ namespace OpenSim.Region.Physics.OdePlugin public ODEDynamics m_vehicle; internal int m_material = (int)Material.Wood; - protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f }; + private float mu; + private float bounce; /// /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. @@ -218,25 +219,23 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override ContactData ContactData + public override void getContactData(ref ContactData cdata) { - get + cdata.mu = mu; + cdata.bounce = bounce; + + if (m_isphysical) { - if (m_isphysical) - { - ODEDynamics veh; - if (_parent != null) - veh = ((OdePrim)_parent).m_vehicle; - else - veh = m_vehicle; + ODEDynamics veh; + if (_parent != null) + veh = ((OdePrim)_parent).m_vehicle; + else + veh = m_vehicle; - if (veh != null) - if (veh.Type != Vehicle.TYPE_NONE && veh.EngineActive) - return new ContactData(0, 0); - } - return primContactData; + if (veh != null && veh.Type != Vehicle.TYPE_NONE) + cdata.mu *= veh.FrictionFactor; } - } + } public override int PhysicsActorType { @@ -745,8 +744,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SetMaterial(int pMaterial) { m_material = pMaterial; - primContactData.mu = _parent_scene.m_materialContactsData[pMaterial].mu; - primContactData.bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + mu = _parent_scene.m_materialContactsData[pMaterial].mu; + bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; } public void setPrimForRemoval() @@ -899,8 +898,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_isSelected = false; m_delaySelect = false; - primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; - primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; CalcPrimBodyData(); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 129db5d..884a5a7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -163,8 +163,6 @@ namespace OpenSim.Region.Physics.OdePlugin const float comumSoftContactERP = 0.1f; const float comumContactCFM = 0.0001f; - float frictionScale = 1.0f; - float frictionMovementMult = 0.3f; float TerrainBounce = 0.1f; @@ -450,33 +448,30 @@ namespace OpenSim.Region.Physics.OdePlugin ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); - m_materialContactsData[(int)Material.Stone].mu = frictionScale * 0.8f; + m_materialContactsData[(int)Material.Stone].mu = 0.8f; m_materialContactsData[(int)Material.Stone].bounce = 0.4f; - m_materialContactsData[(int)Material.Metal].mu = frictionScale * 0.3f; + m_materialContactsData[(int)Material.Metal].mu = 0.3f; m_materialContactsData[(int)Material.Metal].bounce = 0.4f; - m_materialContactsData[(int)Material.Glass].mu = frictionScale * 0.2f; + m_materialContactsData[(int)Material.Glass].mu = 0.2f; m_materialContactsData[(int)Material.Glass].bounce = 0.7f; - m_materialContactsData[(int)Material.Wood].mu = frictionScale * 0.6f; + m_materialContactsData[(int)Material.Wood].mu = 0.6f; m_materialContactsData[(int)Material.Wood].bounce = 0.5f; - m_materialContactsData[(int)Material.Flesh].mu = frictionScale * 0.9f; + m_materialContactsData[(int)Material.Flesh].mu = 0.9f; m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; - m_materialContactsData[(int)Material.Plastic].mu = frictionScale * 0.4f; + m_materialContactsData[(int)Material.Plastic].mu = 0.4f; m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; - m_materialContactsData[(int)Material.Rubber].mu = frictionScale * 0.9f; + m_materialContactsData[(int)Material.Rubber].mu = 0.9f; m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; m_materialContactsData[(int)Material.light].mu = 0.0f; m_materialContactsData[(int)Material.light].bounce = 0.0f; - TerrainFriction *= frictionScale; -// AvatarFriction *= frictionScale; - // Set the gravity,, don't disable things automatically (we set it explicitly on some things) d.WorldSetGravity(world, gravityx, gravityy, gravityz); @@ -562,13 +557,6 @@ namespace OpenSim.Region.Physics.OdePlugin } - /// - /// This is our near callback. A geometry is near a body - /// - /// The space that contains the geoms. Remember, spaces are also geoms - /// a geometry or space - /// another geometry or space - /// private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) { @@ -580,7 +568,13 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } - + /// + /// This is our near callback. A geometry is near a body + /// + /// The space that contains the geoms. Remember, spaces are also geoms + /// a geometry or space + /// another geometry or space + /// private void near(IntPtr space, IntPtr g1, IntPtr g2) { @@ -699,8 +693,8 @@ namespace OpenSim.Region.Physics.OdePlugin // big messy collision analises float mu = 0; float bounce = 0; - ContactData contactdata1; - ContactData contactdata2; + ContactData contactdata1 = new ContactData(0, 0); + ContactData contactdata2 = new ContactData(0, 0); bool erpSoft = false; String name = null; @@ -714,8 +708,9 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: - contactdata1 = p1.ContactData; - contactdata2 = p2.ContactData; + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); + bounce = contactdata1.bounce * contactdata2.bounce; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); @@ -727,8 +722,8 @@ namespace OpenSim.Region.Physics.OdePlugin p2.CollidingObj = true; break; case (int)ActorTypes.Prim: - contactdata1 = p1.ContactData; - contactdata2 = p2.ContactData; + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); bounce = contactdata1.bounce * contactdata2.bounce; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); @@ -749,8 +744,8 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: - contactdata1 = p1.ContactData; - contactdata2 = p2.ContactData; + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); bounce = contactdata1.bounce * contactdata2.bounce; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); @@ -768,8 +763,8 @@ namespace OpenSim.Region.Physics.OdePlugin p1.CollidingObj = true; p2.CollidingObj = true; } - contactdata1 = p1.ContactData; - contactdata2 = p2.ContactData; + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); bounce = contactdata1.bounce * contactdata2.bounce; erpSoft = true; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); @@ -784,7 +779,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (name == "Terrain") { erpSoft = true; - contactdata1 = p1.ContactData; + p1.getContactData(ref contactdata1); bounce = contactdata1.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) @@ -811,7 +806,7 @@ namespace OpenSim.Region.Physics.OdePlugin { erpSoft = true; p2.CollidingGround = true; - contactdata2 = p2.ContactData; + p2.getContactData(ref contactdata2); bounce = contactdata2.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); -- cgit v1.1 From 7377e633c73f7ee34240cb70f0f75fcd9b705168 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 5 Mar 2012 12:37:21 +0000 Subject: update ubitOde --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 4 +- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 355 ++++++++++++++++----- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 15 +- 4 files changed, 297 insertions(+), 78 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 25aa4dc..1a0c2a7 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -72,11 +72,13 @@ namespace OpenSim.Region.Physics.Manager { public float mu; public float bounce; + public bool softcolide; - public ContactData(float _mu, float _bounce) + public ContactData(float _mu, float _bounce, bool _softcolide) { mu = _mu; bounce = _bounce; + softcolide = _softcolide; } } /// diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 94cadb2..9a22331 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -195,6 +195,7 @@ namespace OpenSim.Region.Physics.OdePlugin { cdata.mu = mu; cdata.bounce = bounce; + cdata.softcolide = false; } public override bool Building { get; set; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index db07565..b105f77 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -114,6 +114,9 @@ namespace OpenSim.Region.Physics.OdePlugin // private bool m_collidesLand = true; private bool m_collidesWater; public bool m_returnCollisions; + private bool m_softcolide; + + private bool m_NoColide; // for now only for internal use for bad meshs // Default we're a Geometry private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); @@ -224,6 +227,9 @@ namespace OpenSim.Region.Physics.OdePlugin cdata.mu = mu; cdata.bounce = bounce; + // cdata.softcolide = m_softcolide; + cdata.softcolide = false; + if (m_isphysical) { ODEDynamics veh; @@ -303,7 +309,10 @@ namespace OpenSim.Region.Physics.OdePlugin } if (m_colliderfilter == 0) + { + m_softcolide = false; m_iscolliding = false; + } else m_iscolliding = true; } @@ -859,7 +868,6 @@ namespace OpenSim.Region.Physics.OdePlugin _size = size; - if (!QuaternionIsFinite(rotation)) { rotation = Quaternion.Identity; @@ -890,6 +898,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_iscolliding = false; m_colliderfilter = 0; + m_softcolide = true; + m_NoColide = false; hasOOBoffsetFromMesh = false; _triMeshData = IntPtr.Zero; @@ -1037,34 +1047,42 @@ namespace OpenSim.Region.Physics.OdePlugin if (vertexCount == 0 || indexCount == 0) { - m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z); - _size.X = 0.01f; - _size.Y = 0.01f; - _size.Z = 0.01f; + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", + Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); + mesh.releaseSourceMeshData(); return false; } primOOBoffset = mesh.GetCentroid(); hasOOBoffsetFromMesh = true; - _triMeshData = d.GeomTriMeshDataCreate(); - - d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); - d.GeomTriMeshDataPreprocess(_triMeshData); - mesh.releaseSourceMeshData(); - _parent_scene.waitForSpaceUnlock(m_targetSpace); + IntPtr geo = IntPtr.Zero; + try { - SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); } catch (Exception e) { m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } return false; } + + SetGeom(geo); return true; } @@ -1074,25 +1092,30 @@ namespace OpenSim.Region.Physics.OdePlugin //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); if (prim_geom != IntPtr.Zero) { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + { + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } CalcPrimBodyData(); _parent_scene.geom_name_map[prim_geom] = Name; _parent_scene.actor_name_map[prim_geom] = this; - /* - if (childPrim) - { - if (_parent != null && _parent is OdePrim) - { - OdePrim parent = (OdePrim)_parent; - //Console.WriteLine("SetGeom calls ChildSetGeom"); - parent.ChildSetGeom(this); - } - } - */ } else m_log.Warn("Setting bad Geom"); @@ -1114,10 +1137,13 @@ namespace OpenSim.Region.Physics.OdePlugin bool haveMesh = false; hasOOBoffsetFromMesh = false; + m_NoColide = false; if (_parent_scene.needsMeshing(_pbs)) { haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + if (!haveMesh) + m_NoColide = true; } if (!haveMesh) @@ -1209,12 +1235,46 @@ namespace OpenSim.Region.Physics.OdePlugin { if (!childPrim && !m_isSelected) { - if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) + if (m_isphysical && Body != IntPtr.Zero) { - d.GeomEnable(prim_geom); + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + foreach (OdePrim prm in childrenPrim) - d.GeomEnable(prm.prim_geom); + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prm.prim_geom); + } + } + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prim_geom); + } d.BodyEnable(Body); } } @@ -1227,11 +1287,47 @@ namespace OpenSim.Region.Physics.OdePlugin m_disabled = true; if (!childPrim) { - if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) + if (m_isphysical && Body != IntPtr.Zero) { - d.GeomDisable(prim_geom); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + foreach (OdePrim prm in childrenPrim) - d.GeomDisable(prm.prim_geom); + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prm.prim_geom); + } + } + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prim_geom); + } + d.BodyDisable(Body); } } @@ -1310,8 +1406,6 @@ namespace OpenSim.Region.Physics.OdePlugin continue; } - - DMassCopy(ref prm.primdMass, ref tmpdmass); // apply prim current rotation to inertia @@ -1373,14 +1467,7 @@ namespace OpenSim.Region.Physics.OdePlugin // d.BodySetAngularDampingThreshold(Body, 0.001f); d.BodySetDamping(Body, .002f, .002f); - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - m_collisionscore = 0; -// if (m_targetSpace != _parent_scene.ActiveSpace) - { if (m_targetSpace != IntPtr.Zero) { _parent_scene.waitForSpaceUnlock(m_targetSpace); @@ -1388,10 +1475,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceRemove(m_targetSpace, prim_geom); } -// m_targetSpace = _parent_scene.ActiveSpace; -// d.SpaceAdd(m_targetSpace, prim_geom); - } - if (childrenPrim.Count == 0) { @@ -1419,12 +1502,6 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 ppos = prm._position; d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position - prm.m_collisionCategories |= CollisionCategories.Body; - prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); - prm.m_collisionscore = 0; - if (prm.m_targetSpace != m_targetSpace) { if (prm.m_targetSpace != IntPtr.Zero) @@ -1438,9 +1515,32 @@ namespace OpenSim.Region.Physics.OdePlugin } if (m_isSelected || m_disabled) + { + prm.m_collisionCategories &= ~CollisionCategories.Body; + prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); d.GeomDisable(prm.prim_geom); + } + else + { + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + prm.m_collisionscore = 0; + + if(!m_disabled) + prm.m_disabled = false; - prm.m_disabled = false; _parent_scene.addActivePrim(prm); } } @@ -1453,15 +1553,35 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isSelected || m_disabled) { + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); + d.GeomDisable(prim_geom); d.BodyDisable(Body); } else { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); } + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + m_collisionscore = 0; + + m_softcolide = true; _parent_scene.addActivePrim(this); _parent_scene.addActiveGroups(this); } @@ -1475,8 +1595,16 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); if (prim_geom != IntPtr.Zero) { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } UpdateDataFromGeom(); d.GeomSetBody(prim_geom, IntPtr.Zero); SetInStaticSpace(this); @@ -1489,12 +1617,20 @@ namespace OpenSim.Region.Physics.OdePlugin foreach (OdePrim prm in childrenPrim) { _parent_scene.remActivePrim(prm); - prm.m_collisionCategories &= ~CollisionCategories.Body; - prm.m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; if (prm.prim_geom != IntPtr.Zero) { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } prm.UpdateDataFromGeom(); SetInStaticSpace(prm); } @@ -2044,23 +2180,14 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.Z = _orientation.Z; myrot.W = _orientation.W; d.GeomSetQuaternion(prim_geom, ref myrot); - // _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; + if (!m_isphysical) SetInStaticSpace(this); } if (m_isphysical && Body == IntPtr.Zero) { - /* - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - { - */ MakeBody(); - // } } } @@ -2169,17 +2296,52 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim && Body != IntPtr.Zero) d.BodyDisable(Body); - if (m_delaySelect) + if (m_delaySelect || m_isphysical) { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + if (!childPrim) { foreach (OdePrim prm in childrenPrim) { - d.GeomDisable(prm.prim_geom); + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != null) + { + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prm.prim_geom); + } prm.m_delaySelect = false; } } - d.GeomDisable(prim_geom); + + if (prim_geom != null) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prim_geom); + } + m_delaySelect = false; } else @@ -2192,19 +2354,64 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim && Body != IntPtr.Zero && !m_disabled) d.BodyEnable(Body); + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + if (!childPrim) { foreach (OdePrim prm in childrenPrim) { - if(!prm.m_disabled) + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } d.GeomEnable(prm.prim_geom); + } prm.m_delaySelect = false; + prm.m_softcolide = true; } } - if(!m_disabled) + + if (!m_disabled && prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } d.GeomEnable(prim_geom); + } m_delaySelect = false; + m_softcolide = true; } resetCollisionAccounting(); @@ -2250,6 +2457,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (givefakepos < 0) givefakepos = 0; // changeSelectedStatus(); + m_softcolide = true; resetCollisionAccounting(); } @@ -2302,6 +2510,7 @@ namespace OpenSim.Region.Physics.OdePlugin givefakeori--; if (givefakeori < 0) givefakeori = 0; + m_softcolide = true; resetCollisionAccounting(); } @@ -2372,6 +2581,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (givefakeori < 0) givefakeori = 0; + m_softcolide = true; resetCollisionAccounting(); } @@ -2488,6 +2698,7 @@ namespace OpenSim.Region.Physics.OdePlugin else MakeBody(); + m_softcolide = true; resetCollisionAccounting(); } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 884a5a7..14516f9 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -693,8 +693,8 @@ namespace OpenSim.Region.Physics.OdePlugin // big messy collision analises float mu = 0; float bounce = 0; - ContactData contactdata1 = new ContactData(0, 0); - ContactData contactdata2 = new ContactData(0, 0); + ContactData contactdata1 = new ContactData(0, 0, false); + ContactData contactdata2 = new ContactData(0, 0, false); bool erpSoft = false; String name = null; @@ -718,6 +718,7 @@ namespace OpenSim.Region.Physics.OdePlugin if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; + erpSoft = contactdata1.softcolide | contactdata2.softcolide; p1.CollidingObj = true; p2.CollidingObj = true; break; @@ -732,6 +733,9 @@ namespace OpenSim.Region.Physics.OdePlugin mu *= frictionMovementMult; if (p2.Velocity.LengthSquared() > 0.0f) p2.CollidingObj = true; + + erpSoft = contactdata1.softcolide | contactdata2.softcolide; + dop1foot = true; break; default: @@ -753,6 +757,7 @@ namespace OpenSim.Region.Physics.OdePlugin if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; + erpSoft = contactdata1.softcolide | contactdata2.softcolide; dop2foot = true; if (p1.Velocity.LengthSquared() > 0.0f) p1.CollidingObj = true; @@ -766,7 +771,7 @@ namespace OpenSim.Region.Physics.OdePlugin p1.getContactData(ref contactdata1); p2.getContactData(ref contactdata2); bounce = contactdata1.bounce * contactdata2.bounce; - erpSoft = true; + erpSoft = contactdata1.softcolide | contactdata2.softcolide; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) @@ -778,12 +783,12 @@ namespace OpenSim.Region.Physics.OdePlugin { if (name == "Terrain") { - erpSoft = true; p1.getContactData(ref contactdata1); bounce = contactdata1.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) mu *= frictionMovementMult; + erpSoft = contactdata1.softcolide; p1.CollidingGround = true; } else if (name == "Water") @@ -804,11 +809,11 @@ namespace OpenSim.Region.Physics.OdePlugin { if (p2.PhysicsActorType == (int)ActorTypes.Prim) { - erpSoft = true; p2.CollidingGround = true; p2.getContactData(ref contactdata2); bounce = contactdata2.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + erpSoft = contactdata2.softcolide; if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) mu *= frictionMovementMult; -- cgit v1.1 From 13f340077c20808fe17b5c737a87613156a3b1b3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 9 Mar 2012 04:19:39 +0000 Subject: let chode and ubitode find ode.dll on windows --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 3 +++ OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs | 4 ++++ 2 files changed, 7 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 5f5d547..05f6fae 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -72,6 +72,9 @@ namespace OpenSim.Region.Physics.OdePlugin { if (_mScene == null) { + if (Util.IsWindows()) + Util.LoadArchSpecificWindowsDll("ode.dll"); + // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to // http://opensimulator.org/mantis/view.php?id=2750). d.InitODE(); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs index 215d47a..d32188e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs @@ -52,6 +52,9 @@ namespace OpenSim.Region.Physics.OdePlugin public bool Init() { + if (Util.IsWindows()) + Util.LoadArchSpecificWindowsDll("ode.dll"); + if (d.InitODE2(0) != 0) { if (d.AllocateODEDataForThread(~0U) == 0) @@ -68,6 +71,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_scene == null) { + m_scene = new OdeScene(sceneIdentifier); } return (m_scene); -- cgit v1.1 From 0e5e324a2c0969936946f0432c6783b19fc5e111 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 9 Mar 2012 17:20:49 +0000 Subject: ubitOde bug --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index b105f77..563d527 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1524,17 +1524,19 @@ namespace OpenSim.Region.Physics.OdePlugin { prm.m_collisionCategories |= CollisionCategories.Body; prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + d.GeomEnable(prm.prim_geom); } if (prm.m_NoColide) { d.GeomSetCategoryBits(prm.prim_geom, 0); d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + d.GeomEnable(prm.prim_geom); } else { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); } prm.m_collisionscore = 0; -- cgit v1.1 From 21a76a619f62e76f366d8bfa1f37f776cfb92fa3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 11 Mar 2012 06:32:06 +0000 Subject: initial steps to support physical phantoms --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 8 +++++- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 33 +++++++++++++++++++----- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 2 ++ OpenSim/Region/Physics/Manager/PhysicsScene.cs | 9 ++++--- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 ++++- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 28 ++++++++++++++++++-- 6 files changed, 73 insertions(+), 13 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 3ed3b5a..42a408e 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -131,6 +131,7 @@ namespace OpenSim.Region.Physics.OdePlugin public bool m_disabled; public bool m_taintadd; public bool m_taintselected; + public bool m_taintphantom; public bool m_taintCollidesWater; public uint m_localID; @@ -161,6 +162,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool iscolliding; private bool m_isphysical; + private bool m_isphantom; private bool m_isSelected; private bool m_NoColide; // for now only for internal use for bad meshs @@ -311,8 +313,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintvehicledata = vdata; _parent_scene.AddPhysicsActorTaint(this); } + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) + Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom, CollisionLocker dode, uint localid) { m_localID = localid; ode = dode; @@ -373,6 +376,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_targetSpace = _parent_scene.space; } + m_isphantom = pisPhantom; + m_taintphantom = pisPhantom; + _triMeshData = IntPtr.Zero; m_NoColide = false; diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 05f6fae..ea89d87 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1730,7 +1730,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode, localid); + newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical,false, ode, localid); lock (_prims) _prims.Add(newPrim); @@ -1738,23 +1738,27 @@ namespace OpenSim.Region.Physics.OdePlugin return newPrim; } -/* - private PhysicsActor AddPrim(String name, Vector3 position, PhysicsActor parent, - PrimitiveBaseShape pbs, uint localid, byte[] sdata) + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, bool isphantom, uint localid) { + Vector3 pos = position; + Vector3 siz = size; + Quaternion rot = rotation; OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, parent, pbs, ode, localid, sdata); + newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, isphantom, ode, localid); + lock (_prims) _prims.Add(newPrim); } return newPrim; } -*/ + public void addActivePrim(OdePrim activatePrim) { @@ -1781,6 +1785,23 @@ namespace OpenSim.Region.Physics.OdePlugin return result; } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) + { + PhysicsActor result; + IMesh mesh = null; + + if (needsMeshing(pbs)) + mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true); + + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, isPhantom, localid); + + return result; + } + + + /* public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, uint localid, byte[] sdata) diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 1a0c2a7..46f5b6f 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -170,6 +170,8 @@ namespace OpenSim.Region.Physics.Manager public abstract Vector3 Size { get; set; } + public bool Phantom { get; set; } + public abstract PrimitiveBaseShape Shape { set; } uint m_baseLocalID; diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 0346d4e..eca6a0f 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -122,9 +122,6 @@ namespace OpenSim.Region.Physics.Manager /// public abstract void RemovePrim(PhysicsActor prim); - //public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - // Vector3 size, Quaternion rotation); //To be removed - Actually removed! - public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid); @@ -134,6 +131,12 @@ namespace OpenSim.Region.Physics.Manager return null; } + public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) + { + return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid); + } + public virtual float TimeDilation { get { return 1.0f; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 563d527..63b29ba 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -65,6 +65,7 @@ namespace OpenSim.Region.Physics.OdePlugin private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); private bool m_isphysical; + private bool m_isPhantom; private bool m_fakeisphysical; protected bool m_building; @@ -831,9 +832,10 @@ namespace OpenSim.Region.Physics.OdePlugin public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,uint plocalID) { Name = primName; + LocalID = plocalID; m_vehicle = null; @@ -908,6 +910,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_isSelected = false; m_delaySelect = false; + m_isPhantom = pisPhantom; + mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 14516f9..b111172 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1140,12 +1140,29 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,localID); + + lock (_prims) + _prims.Add(newPrim); + } + return newPrim; + } + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, uint localID) + { + Vector3 pos = position; + Vector3 siz = size; + Quaternion rot = rotation; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,isPhantom,localID); lock (_prims) _prims.Add(newPrim); } - newPrim.LocalID = localID; return newPrim; } @@ -1169,6 +1186,13 @@ namespace OpenSim.Region.Physics.OdePlugin } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) + { + return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, localid); + } + + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { #if SPAM -- cgit v1.1 From 43dc0ee02066a38b46383412b15a69b847975a79 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 11 Mar 2012 07:44:34 +0000 Subject: more phantom physics ( chODE and a fix in manager physicsactor --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 181 ++++++++++++++++--------- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 2 +- 2 files changed, 121 insertions(+), 62 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 42a408e..f9548d2 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -453,6 +453,15 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public override bool Phantom + { + get { return m_isphantom; } + set + { + m_isphantom = value; + } + } + public void setPrimForRemoval() { m_taintremove = true; @@ -896,7 +905,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_NoColide) { d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) + if (m_isphysical && !m_isVolumeDetect) { d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); } @@ -1405,10 +1414,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - } - Body = IntPtr.Zero; } } @@ -1416,7 +1423,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionscore = 0; } - private static Dictionary m_MeshToTriMeshMap = new Dictionary(); +// private static Dictionary m_MeshToTriMeshMap = new Dictionary(); public bool setMesh(OdeScene parent_scene, IMesh mesh) { @@ -1513,12 +1520,17 @@ namespace OpenSim.Region.Physics.OdePlugin } } // + if (m_taintphantom != m_isphantom ) + { + changePhantomStatus(); + }// if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) { changePhysicsStatus(timestep); }// + if (!_size.ApproxEquals(m_taintsize, 0f)) changesize(timestep); // @@ -1540,7 +1552,7 @@ namespace OpenSim.Region.Physics.OdePlugin changedisable(timestep); if (m_taintselected != m_isSelected) - changeSelectedStatus(timestep); + changeSelectedStatus(); if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) changevelocity(timestep); @@ -1571,16 +1583,6 @@ namespace OpenSim.Region.Physics.OdePlugin } - /* obsolete - private void changeAngularLock(float timestep) - { - if (_parent == null) - { - m_angularLock = m_taintAngularLock; - m_angularLockSet = true; - } - } - */ private void changelink(float timestep) { // If the newly set parent is not null @@ -1590,18 +1592,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) { OdePrim obj = (OdePrim)m_taintparent; - //obj.disableBody(); obj.ParentPrim(this); - - /* - if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body) - { - _linkJointGroup = d.JointGroupCreate(0); - m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); - d.JointAttach(m_linkJoint, obj.Body, Body); - d.JointSetFixed(m_linkJoint); - } - */ } } // If the newly set parent is null @@ -1613,16 +1604,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim obj = (OdePrim)_parent; obj.ChildDelink(this); childPrim = false; - //_parent = null; } - - /* - if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) - d.JointGroupDestroy(_linkJointGroup); - - _linkJointGroup = (IntPtr)0; - m_linkJoint = (IntPtr)0; - */ } _parent = m_taintparent; @@ -1672,9 +1654,16 @@ namespace OpenSim.Region.Physics.OdePlugin } foreach (OdePrim prm in childrenPrim) { - prm.m_collisionCategories |= CollisionCategories.Body; - prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - + if (m_isphantom && !prm.m_isVolumeDetect) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = CollisionCategories.Land; + } + else + { + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } if (prm.prim_geom == IntPtr.Zero) { m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); @@ -1728,8 +1717,16 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.addActivePrim(prm); } - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } if (m_NoColide) { @@ -1779,23 +1776,15 @@ namespace OpenSim.Region.Physics.OdePlugin private void ChildSetGeom(OdePrim odePrim) { - //if (m_isphysical && Body != IntPtr.Zero) lock (childrenPrim) { foreach (OdePrim prm in childrenPrim) { - //prm.childPrim = true; prm.disableBody(); - //prm.m_taintparent = null; - //prm._parent = null; - //prm.m_taintPhysics = false; - //prm.m_disabled = true; - //prm.childPrim = false; } } disableBody(); - if (Body != IntPtr.Zero) { _parent_scene.remActivePrim(this); @@ -1808,7 +1797,6 @@ namespace OpenSim.Region.Physics.OdePlugin ParentPrim(prm); } } - } private void ChildDelink(OdePrim odePrim) @@ -1820,11 +1808,6 @@ namespace OpenSim.Region.Physics.OdePlugin { prm.childPrim = true; prm.disableBody(); - //prm.m_taintparent = null; - //prm._parent = null; - //prm.m_taintPhysics = false; - //prm.m_disabled = true; - //prm.childPrim = false; } } disableBody(); @@ -1848,7 +1831,73 @@ namespace OpenSim.Region.Physics.OdePlugin } } - private void changeSelectedStatus(float timestep) + private void changePhantomStatus() + { + m_taintphantom = m_isphantom; + changeSelectedStatus(); + } + +/* not in use + private void SetCollider() + { + SetCollider(m_isSelected, m_isphysical, m_isphantom, m_isSelected); + } + + private void SetCollider(bool sel, bool phys, bool phan, bool vdtc) + { + if (sel) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + } + else + { + if (phan && !vdtc) + { + m_collisionCategories = 0; + if (phys) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (phys) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags; + + if (m_collidesLand) + m_collisionFlags |= CollisionCategories.Land; + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } + } + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (phys) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } + } +*/ + + private void changeSelectedStatus() { if (m_taintselected) { @@ -1901,10 +1950,19 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); } - } else { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics + } + else + { m_collisionCategories = CollisionCategories.Geom; if (m_isphysical) m_collisionCategories |= CollisionCategories.Body; @@ -1915,6 +1973,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionFlags |= CollisionCategories.Land; if (m_collidesWater) m_collisionFlags |= CollisionCategories.Water; + } if (prim_geom != IntPtr.Zero) { @@ -2092,7 +2151,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - changeSelectedStatus(timestep); + changeSelectedStatus(); m_taintadd = false; } @@ -2167,7 +2226,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - changeSelectedStatus(timestep); + changeSelectedStatus(); resetCollisionAccounting(); m_taintposition = _position; @@ -2254,7 +2313,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - changeSelectedStatus(timestep); + changeSelectedStatus(); resetCollisionAccounting(); m_taintPhysics = m_isphysical; @@ -2348,7 +2407,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.geom_name_map[prim_geom] = oldname; - changeSelectedStatus(timestamp); + changeSelectedStatus(); if (childPrim) { if (_parent is OdePrim) @@ -2460,7 +2519,7 @@ namespace OpenSim.Region.Physics.OdePlugin } _parent_scene.geom_name_map[prim_geom] = oldname; - changeSelectedStatus(timestamp); + changeSelectedStatus(); if (childPrim) { if (_parent is OdePrim) diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 46f5b6f..bd80fff 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -170,7 +170,7 @@ namespace OpenSim.Region.Physics.Manager public abstract Vector3 Size { get; set; } - public bool Phantom { get; set; } + public virtual bool Phantom { get; set; } public abstract PrimitiveBaseShape Shape { set; } -- cgit v1.1 From 925295f3ab0b44b656f93c2861b6605de930bcd4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 11 Mar 2012 12:33:59 +0000 Subject: ubitOde also knows a bit more about physical phantom --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 171 ++++++++++++++++++++---- 1 file changed, 142 insertions(+), 29 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 63b29ba..5d8f03c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -65,8 +65,9 @@ namespace OpenSim.Region.Physics.OdePlugin private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); private bool m_isphysical; - private bool m_isPhantom; private bool m_fakeisphysical; + private bool m_isphantom; + private bool m_fakeisphantom; protected bool m_building; private Quaternion m_lastorientation = new Quaternion(); @@ -212,6 +213,19 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public override bool Phantom // this is not reliable for internal use + { + get { return m_fakeisphantom; } + set + { + m_fakeisphantom = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + AddChange(changes.Phantom, value); + } + } + public override bool Building // this is not reliable for internal use { get { return m_building; } @@ -910,7 +924,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_isSelected = false; m_delaySelect = false; - m_isPhantom = pisPhantom; + m_isphantom = pisPhantom; + m_fakeisphantom = pisPhantom; mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; @@ -1241,8 +1256,16 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical && Body != IntPtr.Zero) { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } foreach (OdePrim prm in childrenPrim) { @@ -1496,6 +1519,12 @@ namespace OpenSim.Region.Physics.OdePlugin collide_geom = m_targetSpace; } + if (m_delaySelect) + { + m_isSelected = true; + m_delaySelect = false; + } + lock (childrenPrim) { foreach (OdePrim prm in childrenPrim) @@ -1526,8 +1555,16 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - prm.m_collisionCategories |= CollisionCategories.Body; - prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + if (m_isphantom && !m_isVolumeDetect) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = CollisionCategories.Land; + } + else + { + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } d.GeomEnable(prm.prim_geom); } @@ -1567,8 +1604,16 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); @@ -2275,6 +2320,88 @@ namespace OpenSim.Region.Physics.OdePlugin } } + + private void changePhantomStatus(bool newval) + { + m_isphantom = newval; + + if (m_isSelected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; // should never happen + } + + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } + } + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + if(!m_isSelected) + d.GeomEnable(prm.prim_geom); + } + } + } + + if (!m_disabled && prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + if(!m_isSelected) + d.GeomEnable(prim_geom); + } + } + private void changeSelectedStatus(bool newval) { if (m_lastdoneSelected == newval) @@ -2350,7 +2477,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_delaySelect = false; } - else + else if(!m_isphysical) { m_delaySelect = true; } @@ -2617,30 +2744,12 @@ namespace OpenSim.Region.Physics.OdePlugin if (NewStatus) { if (Body == IntPtr.Zero) - { - /* - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - */ - { - MakeBody(); - } - } + MakeBody(); } else { if (Body != IntPtr.Zero) { - // UpdateChildsfromgeom(); - /* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - */ DestroyBody(); } Stop(); @@ -2724,7 +2833,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_collidesWater = newval; - if (prim_geom != IntPtr.Zero) + if (prim_geom != IntPtr.Zero && !m_isphantom) { if (m_collidesWater) { @@ -3496,6 +3605,10 @@ namespace OpenSim.Region.Physics.OdePlugin changeVolumedetetion((bool)arg); break; + case changes.Phantom: + changePhantomStatus((bool)arg); + break; + case changes.Physical: changePhysicsStatus((bool)arg); break; -- cgit v1.1 From 2a17dafa0463ff5699adca3f5cfe56a91de50e04 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 12 Mar 2012 06:52:03 +0000 Subject: missing file in ubitODE --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 1 + 1 file changed, 1 insertion(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index b111172..612eafd 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -128,6 +128,7 @@ namespace OpenSim.Region.Physics.OdePlugin VolumeDtc, Physical, + Phantom, Selected, disabled, building, -- cgit v1.1 From b5e172677dfed6a9b6ec60c86aea687f46543ec7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 12 Mar 2012 07:46:20 +0000 Subject: ubitode prim select was not doing phantom case --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 5d8f03c..39b89d3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2487,14 +2487,25 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim && Body != IntPtr.Zero && !m_disabled) d.BodyEnable(Body); - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if(m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } if (!childPrim) { -- cgit v1.1 From 41a0c850f887d72ff64e9e616ea4bda25fd85de7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 17 Mar 2012 09:27:56 +0000 Subject: added a new UbitMeshing module so i can mess it... --- OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs | 436 ++++ OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 401 ++++ OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 762 +++++++ OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs | 2284 +++++++++++++++++++++ OpenSim/Region/Physics/UbitMeshing/SculptMap.cs | 197 ++ OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs | 646 ++++++ 6 files changed, 4726 insertions(+) create mode 100644 OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs create mode 100644 OpenSim/Region/Physics/UbitMeshing/Mesh.cs create mode 100644 OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs create mode 100644 OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs create mode 100644 OpenSim/Region/Physics/UbitMeshing/SculptMap.cs create mode 100644 OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs b/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs new file mode 100644 index 0000000..8cd8dcf --- /dev/null +++ b/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs @@ -0,0 +1,436 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Globalization; +using OpenMetaverse; +using OpenSim.Region.Physics.Manager; +using OpenSim.Region.Physics.Meshing; + +public class Vertex : IComparable +{ + Vector3 vector; + + public float X + { + get { return vector.X; } + set { vector.X = value; } + } + + public float Y + { + get { return vector.Y; } + set { vector.Y = value; } + } + + public float Z + { + get { return vector.Z; } + set { vector.Z = value; } + } + + public Vertex(float x, float y, float z) + { + vector.X = x; + vector.Y = y; + vector.Z = z; + } + + public Vertex normalize() + { + float tlength = vector.Length(); + if (tlength != 0f) + { + float mul = 1.0f / tlength; + return new Vertex(vector.X * mul, vector.Y * mul, vector.Z * mul); + } + else + { + return new Vertex(0f, 0f, 0f); + } + } + + public Vertex cross(Vertex v) + { + return new Vertex(vector.Y * v.Z - vector.Z * v.Y, vector.Z * v.X - vector.X * v.Z, vector.X * v.Y - vector.Y * v.X); + } + + // disable warning: mono compiler moans about overloading + // operators hiding base operator but should not according to C# + // language spec +#pragma warning disable 0108 + public static Vertex operator *(Vertex v, Quaternion q) + { + // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ + + Vertex v2 = new Vertex(0f, 0f, 0f); + + v2.X = q.W * q.W * v.X + + 2f * q.Y * q.W * v.Z - + 2f * q.Z * q.W * v.Y + + q.X * q.X * v.X + + 2f * q.Y * q.X * v.Y + + 2f * q.Z * q.X * v.Z - + q.Z * q.Z * v.X - + q.Y * q.Y * v.X; + + v2.Y = + 2f * q.X * q.Y * v.X + + q.Y * q.Y * v.Y + + 2f * q.Z * q.Y * v.Z + + 2f * q.W * q.Z * v.X - + q.Z * q.Z * v.Y + + q.W * q.W * v.Y - + 2f * q.X * q.W * v.Z - + q.X * q.X * v.Y; + + v2.Z = + 2f * q.X * q.Z * v.X + + 2f * q.Y * q.Z * v.Y + + q.Z * q.Z * v.Z - + 2f * q.W * q.Y * v.X - + q.Y * q.Y * v.Z + + 2f * q.W * q.X * v.Y - + q.X * q.X * v.Z + + q.W * q.W * v.Z; + + return v2; + } + + public static Vertex operator +(Vertex v1, Vertex v2) + { + return new Vertex(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z); + } + + public static Vertex operator -(Vertex v1, Vertex v2) + { + return new Vertex(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); + } + + public static Vertex operator *(Vertex v1, Vertex v2) + { + return new Vertex(v1.X * v2.X, v1.Y * v2.Y, v1.Z * v2.Z); + } + + public static Vertex operator +(Vertex v1, float am) + { + v1.X += am; + v1.Y += am; + v1.Z += am; + return v1; + } + + public static Vertex operator -(Vertex v1, float am) + { + v1.X -= am; + v1.Y -= am; + v1.Z -= am; + return v1; + } + + public static Vertex operator *(Vertex v1, float am) + { + v1.X *= am; + v1.Y *= am; + v1.Z *= am; + return v1; + } + + public static Vertex operator /(Vertex v1, float am) + { + if (am == 0f) + { + return new Vertex(0f,0f,0f); + } + float mul = 1.0f / am; + v1.X *= mul; + v1.Y *= mul; + v1.Z *= mul; + return v1; + } +#pragma warning restore 0108 + + + public float dot(Vertex v) + { + return X * v.X + Y * v.Y + Z * v.Z; + } + + public Vertex(Vector3 v) + { + vector = v; + } + + public Vertex Clone() + { + return new Vertex(X, Y, Z); + } + + public static Vertex FromAngle(double angle) + { + return new Vertex((float) Math.Cos(angle), (float) Math.Sin(angle), 0.0f); + } + + public float Length() + { + return vector.Length(); + } + + public virtual bool Equals(Vertex v, float tolerance) + { + Vertex diff = this - v; + float d = diff.Length(); + if (d < tolerance) + return true; + + return false; + } + + + public int CompareTo(Vertex other) + { + if (X < other.X) + return -1; + + if (X > other.X) + return 1; + + if (Y < other.Y) + return -1; + + if (Y > other.Y) + return 1; + + if (Z < other.Z) + return -1; + + if (Z > other.Z) + return 1; + + return 0; + } + + public static bool operator >(Vertex me, Vertex other) + { + return me.CompareTo(other) > 0; + } + + public static bool operator <(Vertex me, Vertex other) + { + return me.CompareTo(other) < 0; + } + + public String ToRaw() + { + // Why this stuff with the number formatter? + // Well, the raw format uses the english/US notation of numbers + // where the "," separates groups of 1000 while the "." marks the border between 1 and 10E-1. + // The german notation uses these characters exactly vice versa! + // The Float.ToString() routine is a localized one, giving different results depending on the country + // settings your machine works with. Unusable for a machine readable file format :-( + NumberFormatInfo nfi = new NumberFormatInfo(); + nfi.NumberDecimalSeparator = "."; + nfi.NumberDecimalDigits = 3; + + String s1 = X.ToString("N2", nfi) + " " + Y.ToString("N2", nfi) + " " + Z.ToString("N2", nfi); + + return s1; + } +} + +public class Triangle +{ + public Vertex v1; + public Vertex v2; + public Vertex v3; + + private float radius_square; + private float cx; + private float cy; + + public Triangle(Vertex _v1, Vertex _v2, Vertex _v3) + { + v1 = _v1; + v2 = _v2; + v3 = _v3; + + CalcCircle(); + } + + public bool isInCircle(float x, float y) + { + float dx, dy; + float dd; + + dx = x - cx; + dy = y - cy; + + dd = dx*dx + dy*dy; + if (dd < radius_square) + return true; + else + return false; + } + + public bool isDegraded() + { + // This means, the vertices of this triangle are somewhat strange. + // They either line up or at least two of them are identical + return (radius_square == 0.0); + } + + private void CalcCircle() + { + // Calculate the center and the radius of a circle given by three points p1, p2, p3 + // It is assumed, that the triangles vertices are already set correctly + double p1x, p2x, p1y, p2y, p3x, p3y; + + // Deviation of this routine: + // A circle has the general equation (M-p)^2=r^2, where M and p are vectors + // this gives us three equations f(p)=r^2, each for one point p1, p2, p3 + // putting respectively two equations together gives two equations + // f(p1)=f(p2) and f(p1)=f(p3) + // bringing all constant terms to one side brings them to the form + // M*v1=c1 resp.M*v2=c2 where v1=(p1-p2) and v2=(p1-p3) (still vectors) + // and c1, c2 are scalars (Naming conventions like the variables below) + // Now using the equations that are formed by the components of the vectors + // and isolate Mx lets you make one equation that only holds My + // The rest is straight forward and eaasy :-) + // + + /* helping variables for temporary results */ + double c1, c2; + double v1x, v1y, v2x, v2y; + + double z, n; + + double rx, ry; + + // Readout the three points, the triangle consists of + p1x = v1.X; + p1y = v1.Y; + + p2x = v2.X; + p2y = v2.Y; + + p3x = v3.X; + p3y = v3.Y; + + /* calc helping values first */ + c1 = (p1x*p1x + p1y*p1y - p2x*p2x - p2y*p2y)/2; + c2 = (p1x*p1x + p1y*p1y - p3x*p3x - p3y*p3y)/2; + + v1x = p1x - p2x; + v1y = p1y - p2y; + + v2x = p1x - p3x; + v2y = p1y - p3y; + + z = (c1*v2x - c2*v1x); + n = (v1y*v2x - v2y*v1x); + + if (n == 0.0) // This is no triangle, i.e there are (at least) two points at the same location + { + radius_square = 0.0f; + return; + } + + cy = (float) (z/n); + + if (v2x != 0.0) + { + cx = (float) ((c2 - v2y*cy)/v2x); + } + else if (v1x != 0.0) + { + cx = (float) ((c1 - v1y*cy)/v1x); + } + else + { + Debug.Assert(false, "Malformed triangle"); /* Both terms zero means nothing good */ + } + + rx = (p1x - cx); + ry = (p1y - cy); + + radius_square = (float) (rx*rx + ry*ry); + } + + public override String ToString() + { + NumberFormatInfo nfi = new NumberFormatInfo(); + nfi.CurrencyDecimalDigits = 2; + nfi.CurrencyDecimalSeparator = "."; + + String s1 = "<" + v1.X.ToString(nfi) + "," + v1.Y.ToString(nfi) + "," + v1.Z.ToString(nfi) + ">"; + String s2 = "<" + v2.X.ToString(nfi) + "," + v2.Y.ToString(nfi) + "," + v2.Z.ToString(nfi) + ">"; + String s3 = "<" + v3.X.ToString(nfi) + "," + v3.Y.ToString(nfi) + "," + v3.Z.ToString(nfi) + ">"; + + return s1 + ";" + s2 + ";" + s3; + } + + public Vector3 getNormal() + { + // Vertices + + // Vectors for edges + Vector3 e1; + Vector3 e2; + + e1 = new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); + e2 = new Vector3(v1.X - v3.X, v1.Y - v3.Y, v1.Z - v3.Z); + + // Cross product for normal + Vector3 n = Vector3.Cross(e1, e2); + + // Length + float l = n.Length(); + + // Normalized "normal" + n = n/l; + + return n; + } + + public void invertNormal() + { + Vertex vt; + vt = v1; + v1 = v2; + v2 = vt; + } + + // Dumps a triangle in the "raw faces" format, blender can import. This is for visualisation and + // debugging purposes + public String ToStringRaw() + { + String output = v1.ToRaw() + " " + v2.ToRaw() + " " + v3.ToRaw(); + return output; + } +} diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs new file mode 100644 index 0000000..c715642 --- /dev/null +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -0,0 +1,401 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.IO; +using System.Runtime.InteropServices; +using OpenSim.Region.Physics.Manager; +using PrimMesher; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.Meshing +{ + public class Mesh : IMesh + { + private Dictionary m_vertices; + private List m_triangles; + GCHandle m_pinnedVertexes; + GCHandle m_pinnedIndex; + IntPtr m_verticesPtr = IntPtr.Zero; + int m_vertexCount = 0; + IntPtr m_indicesPtr = IntPtr.Zero; + int m_indexCount = 0; + public float[] m_normals; + Vector3 _centroid; + int _centroidDiv; + + private class vertexcomp : IEqualityComparer + { + public bool Equals(Vertex v1, Vertex v2) + { + if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z) + return true; + else + return false; + } + public int GetHashCode(Vertex v) + { + int a = v.X.GetHashCode(); + int b = v.Y.GetHashCode(); + int c = v.Z.GetHashCode(); + return (a << 16) ^ (b << 8) ^ c; + } + + } + + public Mesh() + { + vertexcomp vcomp = new vertexcomp(); + + m_vertices = new Dictionary(vcomp); + m_triangles = new List(); + _centroid = Vector3.Zero; + _centroidDiv = 0; + } + + public Mesh Clone() + { + Mesh result = new Mesh(); + + foreach (Triangle t in m_triangles) + { + result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); + } + result._centroid = _centroid; + result._centroidDiv = _centroidDiv; + return result; + } + + public void Add(Triangle triangle) + { + if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to Add to a pinned Mesh"); + // If a vertex of the triangle is not yet in the vertices list, + // add it and set its index to the current index count + // vertex == seems broken + // skip colapsed triangles + if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) + || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) + || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z) + ) + { + return; + } + + if (m_vertices.Count == 0) + { + _centroidDiv = 0; + _centroid = Vector3.Zero; + } + + if (!m_vertices.ContainsKey(triangle.v1)) + { + m_vertices[triangle.v1] = m_vertices.Count; + _centroid.X += triangle.v1.X; + _centroid.Y += triangle.v1.Y; + _centroid.Z += triangle.v1.Z; + _centroidDiv++; + } + if (!m_vertices.ContainsKey(triangle.v2)) + { + m_vertices[triangle.v2] = m_vertices.Count; + _centroid.X += triangle.v2.X; + _centroid.Y += triangle.v2.Y; + _centroid.Z += triangle.v2.Z; + _centroidDiv++; + } + if (!m_vertices.ContainsKey(triangle.v3)) + { + m_vertices[triangle.v3] = m_vertices.Count; + _centroid.X += triangle.v3.X; + _centroid.Y += triangle.v3.Y; + _centroid.Z += triangle.v3.Z; + _centroidDiv++; + } + m_triangles.Add(triangle); + } + + public Vector3 GetCentroid() + { + if (_centroidDiv > 0) + return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv); + else + return Vector3.Zero; + } + + public void CalcNormals() + { + int iTriangles = m_triangles.Count; + + this.m_normals = new float[iTriangles * 3]; + + int i = 0; + foreach (Triangle t in m_triangles) + { + float ux, uy, uz; + float vx, vy, vz; + float wx, wy, wz; + + ux = t.v1.X; + uy = t.v1.Y; + uz = t.v1.Z; + + vx = t.v2.X; + vy = t.v2.Y; + vz = t.v2.Z; + + wx = t.v3.X; + wy = t.v3.Y; + wz = t.v3.Z; + + + // Vectors for edges + float e1x, e1y, e1z; + float e2x, e2y, e2z; + + e1x = ux - vx; + e1y = uy - vy; + e1z = uz - vz; + + e2x = ux - wx; + e2y = uy - wy; + e2z = uz - wz; + + + // Cross product for normal + float nx, ny, nz; + nx = e1y * e2z - e1z * e2y; + ny = e1z * e2x - e1x * e2z; + nz = e1x * e2y - e1y * e2x; + + // Length + float l = (float)Math.Sqrt(nx * nx + ny * ny + nz * nz); + float lReciprocal = 1.0f / l; + + // Normalized "normal" + //nx /= l; + //ny /= l; + //nz /= l; + + m_normals[i] = nx * lReciprocal; + m_normals[i + 1] = ny * lReciprocal; + m_normals[i + 2] = nz * lReciprocal; + + i += 3; + } + } + + public List getVertexList() + { + List result = new List(); + foreach (Vertex v in m_vertices.Keys) + { + result.Add(new Vector3(v.X, v.Y, v.Z)); + } + return result; + } + + private float[] getVertexListAsFloat() + { + if (m_vertices == null) + throw new NotSupportedException(); + float[] result = new float[m_vertices.Count * 3]; + foreach (KeyValuePair kvp in m_vertices) + { + Vertex v = kvp.Key; + int i = kvp.Value; + result[3 * i + 0] = v.X; + result[3 * i + 1] = v.Y; + result[3 * i + 2] = v.Z; + } + return result; + } + + public float[] getVertexListAsFloatLocked() + { + if (m_pinnedVertexes.IsAllocated) + return (float[])(m_pinnedVertexes.Target); + + float[] result = getVertexListAsFloat(); + m_pinnedVertexes = GCHandle.Alloc(result, GCHandleType.Pinned); + // Inform the garbage collector of this unmanaged allocation so it can schedule + // the next GC round more intelligently + GC.AddMemoryPressure(Buffer.ByteLength(result)); + + return result; + } + + public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount) + { + // A vertex is 3 floats + vertexStride = 3 * sizeof(float); + + // If there isn't an unmanaged array allocated yet, do it now + if (m_verticesPtr == IntPtr.Zero) + { + float[] vertexList = getVertexListAsFloat(); + // Each vertex is 3 elements (floats) + m_vertexCount = vertexList.Length / 3; + int byteCount = m_vertexCount * vertexStride; + m_verticesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount); + System.Runtime.InteropServices.Marshal.Copy(vertexList, 0, m_verticesPtr, m_vertexCount * 3); + } + vertices = m_verticesPtr; + vertexCount = m_vertexCount; + } + + public int[] getIndexListAsInt() + { + if (m_triangles == null) + throw new NotSupportedException(); + int[] result = new int[m_triangles.Count * 3]; + for (int i = 0; i < m_triangles.Count; i++) + { + Triangle t = m_triangles[i]; + result[3 * i + 0] = m_vertices[t.v1]; + result[3 * i + 1] = m_vertices[t.v2]; + result[3 * i + 2] = m_vertices[t.v3]; + } + return result; + } + + /// + /// creates a list of index values that defines triangle faces. THIS METHOD FREES ALL NON-PINNED MESH DATA + /// + /// + public int[] getIndexListAsIntLocked() + { + if (m_pinnedIndex.IsAllocated) + return (int[])(m_pinnedIndex.Target); + + int[] result = getIndexListAsInt(); + m_pinnedIndex = GCHandle.Alloc(result, GCHandleType.Pinned); + // Inform the garbage collector of this unmanaged allocation so it can schedule + // the next GC round more intelligently + GC.AddMemoryPressure(Buffer.ByteLength(result)); + + return result; + } + + public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount) + { + // If there isn't an unmanaged array allocated yet, do it now + if (m_indicesPtr == IntPtr.Zero) + { + int[] indexList = getIndexListAsInt(); + m_indexCount = indexList.Length; + int byteCount = m_indexCount * sizeof(int); + m_indicesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount); + System.Runtime.InteropServices.Marshal.Copy(indexList, 0, m_indicesPtr, m_indexCount); + } + // A triangle is 3 ints (indices) + triStride = 3 * sizeof(int); + indices = m_indicesPtr; + indexCount = m_indexCount; + } + + public void releasePinned() + { + if (m_pinnedVertexes.IsAllocated) + m_pinnedVertexes.Free(); + if (m_pinnedIndex.IsAllocated) + m_pinnedIndex.Free(); + if (m_verticesPtr != IntPtr.Zero) + { + System.Runtime.InteropServices.Marshal.FreeHGlobal(m_verticesPtr); + m_verticesPtr = IntPtr.Zero; + } + if (m_indicesPtr != IntPtr.Zero) + { + System.Runtime.InteropServices.Marshal.FreeHGlobal(m_indicesPtr); + m_indicesPtr = IntPtr.Zero; + } + } + + /// + /// frees up the source mesh data to minimize memory - call this method after calling get*Locked() functions + /// + public void releaseSourceMeshData() + { + m_triangles = null; + m_vertices = null; + } + + public void Append(IMesh newMesh) + { + if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to Append to a pinned Mesh"); + + if (!(newMesh is Mesh)) + return; + + foreach (Triangle t in ((Mesh)newMesh).m_triangles) + Add(t); + } + + // Do a linear transformation of mesh. + public void TransformLinear(float[,] matrix, float[] offset) + { + if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh"); + + foreach (Vertex v in m_vertices.Keys) + { + if (v == null) + continue; + float x, y, z; + x = v.X*matrix[0, 0] + v.Y*matrix[1, 0] + v.Z*matrix[2, 0]; + y = v.X*matrix[0, 1] + v.Y*matrix[1, 1] + v.Z*matrix[2, 1]; + z = v.X*matrix[0, 2] + v.Y*matrix[1, 2] + v.Z*matrix[2, 2]; + v.X = x + offset[0]; + v.Y = y + offset[1]; + v.Z = z + offset[2]; + } + } + + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + foreach (Triangle t in m_triangles) + { + String s = t.ToStringRaw(); + sw.WriteLine(s); + } + sw.Close(); + } + + public void TrimExcess() + { + m_triangles.TrimExcess(); + } + } +} diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs new file mode 100644 index 0000000..c9c52c0 --- /dev/null +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -0,0 +1,762 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +//#define SPAM + +using System; +using System.Collections.Generic; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; +using OpenMetaverse.StructuredData; +using System.Drawing; +using System.Drawing.Imaging; +using System.IO.Compression; +using PrimMesher; +using log4net; +using Nini.Config; +using System.Reflection; +using System.IO; +using ComponentAce.Compression.Libs.zlib; + +namespace OpenSim.Region.Physics.Meshing +{ + public class MeshmerizerPlugin : IMeshingPlugin + { + public MeshmerizerPlugin() + { + } + + public string GetName() + { + return "UbitMeshmerizer"; + } + + public IMesher GetMesher(IConfigSource config) + { + return new Meshmerizer(config); + } + } + + public class Meshmerizer : IMesher + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + // Setting baseDir to a path will enable the dumping of raw files + // raw files can be imported by blender so a visual inspection of the results can be done +#if SPAM + const string baseDir = "rawFiles"; +#else + private const string baseDir = null; //"rawFiles"; +#endif + + private bool cacheSculptMaps = true; + private bool cacheSculptAlphaMaps = true; + + private string decodedSculptMapPath = null; + private bool useMeshiesPhysicsMesh = false; + + private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh + + private Dictionary m_uniqueMeshes = new Dictionary(); + + public Meshmerizer(IConfigSource config) + { + IConfig start_config = config.Configs["Startup"]; + IConfig mesh_config = config.Configs["Mesh"]; + + decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); + + cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + cacheSculptAlphaMaps = false; + } + else + cacheSculptAlphaMaps = cacheSculptMaps; + + if(mesh_config != null) + useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); + + try + { + if (!Directory.Exists(decodedSculptMapPath)) + Directory.CreateDirectory(decodedSculptMapPath); + } + catch (Exception e) + { + m_log.WarnFormat("[SCULPT]: Unable to create {0} directory: ", decodedSculptMapPath, e.Message); + } + } + + /// + /// creates a simple box mesh of the specified size. This mesh is of very low vertex count and may + /// be useful as a backup proxy when level of detail is not needed or when more complex meshes fail + /// for some reason + /// + /// + /// + /// + /// + /// + /// + /// + private static Mesh CreateSimpleBoxMesh(float minX, float maxX, float minY, float maxY, float minZ, float maxZ) + { + Mesh box = new Mesh(); + List vertices = new List(); + // bottom + + vertices.Add(new Vertex(minX, maxY, minZ)); + vertices.Add(new Vertex(maxX, maxY, minZ)); + vertices.Add(new Vertex(maxX, minY, minZ)); + vertices.Add(new Vertex(minX, minY, minZ)); + + box.Add(new Triangle(vertices[0], vertices[1], vertices[2])); + box.Add(new Triangle(vertices[0], vertices[2], vertices[3])); + + // top + + vertices.Add(new Vertex(maxX, maxY, maxZ)); + vertices.Add(new Vertex(minX, maxY, maxZ)); + vertices.Add(new Vertex(minX, minY, maxZ)); + vertices.Add(new Vertex(maxX, minY, maxZ)); + + box.Add(new Triangle(vertices[4], vertices[5], vertices[6])); + box.Add(new Triangle(vertices[4], vertices[6], vertices[7])); + + // sides + + box.Add(new Triangle(vertices[5], vertices[0], vertices[3])); + box.Add(new Triangle(vertices[5], vertices[3], vertices[6])); + + box.Add(new Triangle(vertices[1], vertices[0], vertices[5])); + box.Add(new Triangle(vertices[1], vertices[5], vertices[4])); + + box.Add(new Triangle(vertices[7], vertices[1], vertices[4])); + box.Add(new Triangle(vertices[7], vertices[2], vertices[1])); + + box.Add(new Triangle(vertices[3], vertices[2], vertices[7])); + box.Add(new Triangle(vertices[3], vertices[7], vertices[6])); + + return box; + } + + /// + /// Creates a simple bounding box mesh for a complex input mesh + /// + /// + /// + private static Mesh CreateBoundingBoxMesh(Mesh meshIn) + { + float minX = float.MaxValue; + float maxX = float.MinValue; + float minY = float.MaxValue; + float maxY = float.MinValue; + float minZ = float.MaxValue; + float maxZ = float.MinValue; + + foreach (Vector3 v in meshIn.getVertexList()) + { + if (v.X < minX) minX = v.X; + if (v.Y < minY) minY = v.Y; + if (v.Z < minZ) minZ = v.Z; + + if (v.X > maxX) maxX = v.X; + if (v.Y > maxY) maxY = v.Y; + if (v.Z > maxZ) maxZ = v.Z; + } + + return CreateSimpleBoxMesh(minX, maxX, minY, maxY, minZ, maxZ); + } + + private void ReportPrimError(string message, string primName, PrimMesh primMesh) + { + m_log.Error(message); + m_log.Error("\nPrim Name: " + primName); + m_log.Error("****** PrimMesh Parameters ******\n" + primMesh.ParamsToDisplayString()); + } + + /// + /// Add a submesh to an existing list of coords and faces. + /// + /// + /// Size of entire object + /// + /// + private void AddSubMesh(OSDMap subMeshData, Vector3 size, List coords, List faces) + { + // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap)); + + // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level + // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no + // geometry for this submesh. + if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"])) + return; + + OpenMetaverse.Vector3 posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3(); + OpenMetaverse.Vector3 posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3(); + ushort faceIndexOffset = (ushort)coords.Count; + + byte[] posBytes = subMeshData["Position"].AsBinary(); + for (int i = 0; i < posBytes.Length; i += 6) + { + ushort uX = Utils.BytesToUInt16(posBytes, i); + ushort uY = Utils.BytesToUInt16(posBytes, i + 2); + ushort uZ = Utils.BytesToUInt16(posBytes, i + 4); + + Coord c = new Coord( + Utils.UInt16ToFloat(uX, posMin.X, posMax.X) * size.X, + Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y) * size.Y, + Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z) * size.Z); + + coords.Add(c); + } + + byte[] triangleBytes = subMeshData["TriangleList"].AsBinary(); + for (int i = 0; i < triangleBytes.Length; i += 6) + { + ushort v1 = (ushort)(Utils.BytesToUInt16(triangleBytes, i) + faceIndexOffset); + ushort v2 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 2) + faceIndexOffset); + ushort v3 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 4) + faceIndexOffset); + Face f = new Face(v1, v2, v3); + faces.Add(f); + } + } + + /// + /// Create a physics mesh from data that comes with the prim. The actual data used depends on the prim type. + /// + /// + /// + /// + /// + /// + private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, Vector3 size, float lod) + { +// m_log.DebugFormat( +// "[MESH]: Creating physics proxy for {0}, shape {1}", +// primName, (OpenMetaverse.SculptType)primShape.SculptType); + + List coords; + List faces; + + if (primShape.SculptEntry) + { + if (((OpenMetaverse.SculptType)primShape.SculptType) == SculptType.Mesh) + { + if (!useMeshiesPhysicsMesh) + return null; + + if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, size, out coords, out faces)) + return null; + } + else + { + if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) + return null; + // Remove the reference to any JPEG2000 sculpt data so it can be GCed + // don't loose it + // primShape.SculptData = Utils.EmptyBytes; + } +// primShape.SculptDataLoaded = true; + } + else + { + if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) + return null; + } + // keep compatible + primShape.SculptData = Utils.EmptyBytes; + + int numCoords = coords.Count; + int numFaces = faces.Count; + + // Create the list of vertices + List vertices = new List(); + for (int i = 0; i < numCoords; i++) + { + Coord c = coords[i]; + vertices.Add(new Vertex(c.X, c.Y, c.Z)); + } + + Mesh mesh = new Mesh(); + // Add the corresponding triangles to the mesh + for (int i = 0; i < numFaces; i++) + { + Face f = faces[i]; + mesh.Add(new Triangle(vertices[f.v1], vertices[f.v2], vertices[f.v3])); + } + + return mesh; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the mesh data the accompanies a prim. + /// + /// + /// + /// + /// Coords are added to this list by the method. + /// Faces are added to this list by the method. + /// true if coords and faces were successfully generated, false if not + private bool GenerateCoordsAndFacesFromPrimMeshData( + string primName, PrimitiveBaseShape primShape, Vector3 size, out List coords, out List faces) + { +// m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); + + coords = new List(); + faces = new List(); + OSD meshOsd = null; + + if (primShape.SculptData.Length <= 0) + { + m_log.ErrorFormat("[MESH]: asset data for {0} is zero length", primName); + return false; + } + + long start = 0; + using (MemoryStream data = new MemoryStream(primShape.SculptData)) + { + try + { + OSD osd = OSDParser.DeserializeLLSDBinary(data); + if (osd is OSDMap) + meshOsd = (OSDMap)osd; + else + { + m_log.Warn("[Mesh}: unable to cast mesh asset to OSDMap"); + return false; + } + } + catch (Exception e) + { + m_log.Error("[MESH]: Exception deserializing mesh asset header:" + e.ToString()); + } + + start = data.Position; + } + + if (meshOsd is OSDMap) + { + OSDMap physicsParms = null; + OSDMap map = (OSDMap)meshOsd; + if (map.ContainsKey("physics_shape")) + physicsParms = (OSDMap)map["physics_shape"]; // old asset format + else if (map.ContainsKey("physics_mesh")) + physicsParms = (OSDMap)map["physics_mesh"]; // new asset format + + if (physicsParms == null) + { + m_log.Warn("[MESH]: no recognized physics mesh found in mesh asset"); + return false; + } + + int physOffset = physicsParms["offset"].AsInteger() + (int)start; + int physSize = physicsParms["size"].AsInteger(); + + if (physOffset < 0 || physSize == 0) + return false; // no mesh data in asset + + OSD decodedMeshOsd = new OSD(); + byte[] meshBytes = new byte[physSize]; + System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize); +// byte[] decompressed = new byte[physSize * 5]; + try + { + using (MemoryStream inMs = new MemoryStream(meshBytes)) + { + using (MemoryStream outMs = new MemoryStream()) + { + using (ZOutputStream zOut = new ZOutputStream(outMs)) + { + byte[] readBuffer = new byte[2048]; + int readLen = 0; + while ((readLen = inMs.Read(readBuffer, 0, readBuffer.Length)) > 0) + { + zOut.Write(readBuffer, 0, readLen); + } + zOut.Flush(); + outMs.Seek(0, SeekOrigin.Begin); + + byte[] decompressedBuf = outMs.GetBuffer(); + + decodedMeshOsd = OSDParser.DeserializeLLSDBinary(decompressedBuf); + } + } + } + } + catch (Exception e) + { + m_log.Error("[MESH]: exception decoding physical mesh: " + e.ToString()); + return false; + } + + OSDArray decodedMeshOsdArray = null; + + // physics_shape is an array of OSDMaps, one for each submesh + if (decodedMeshOsd is OSDArray) + { +// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); + + decodedMeshOsdArray = (OSDArray)decodedMeshOsd; + foreach (OSD subMeshOsd in decodedMeshOsdArray) + { + if (subMeshOsd is OSDMap) + AddSubMesh(subMeshOsd as OSDMap, size, coords, faces); + } + } + } + + return true; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the sculpt data the accompanies a prim. + /// + /// + /// + /// + /// + /// Coords are added to this list by the method. + /// Faces are added to this list by the method. + /// true if coords and faces were successfully generated, false if not + private bool GenerateCoordsAndFacesFromPrimSculptData( + string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, out List coords, out List faces) + { + coords = new List(); + faces = new List(); + PrimMesher.SculptMesh sculptMesh; + Image idata = null; + string decodedSculptFileName = ""; + + if (cacheSculptMaps && primShape.SculptTexture != UUID.Zero) + { + decodedSculptFileName = System.IO.Path.Combine(decodedSculptMapPath, "smap_" + primShape.SculptTexture.ToString()); + try + { + if (File.Exists(decodedSculptFileName)) + { + idata = Image.FromFile(decodedSculptFileName); + } + } + catch (Exception e) + { + m_log.Error("[SCULPT]: unable to load cached sculpt map " + decodedSculptFileName + " " + e.Message); + + } + //if (idata != null) + // m_log.Debug("[SCULPT]: loaded cached map asset for map ID: " + primShape.SculptTexture.ToString()); + } + + if (idata == null) + { + if (primShape.SculptData == null || primShape.SculptData.Length == 0) + return false; + + try + { + OpenMetaverse.Imaging.ManagedImage unusedData; + OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata); + + if (idata == null) + { + // In some cases it seems that the decode can return a null bitmap without throwing + // an exception + m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap. Ignoring.", primName); + + return false; + } + + unusedData = null; + + //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData); + + if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0))) + // don't cache images with alpha channel in linux since mono can't load them correctly) + { + try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); } + catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); } + } + } + catch (DllNotFoundException) + { + m_log.Error("[PHYSICS]: OpenJpeg is not installed correctly on this system. Physics Proxy generation failed. Often times this is because of an old version of GLIBC. You must have version 2.4 or above!"); + return false; + } + catch (IndexOutOfRangeException) + { + m_log.Error("[PHYSICS]: OpenJpeg was unable to decode this. Physics Proxy generation failed"); + return false; + } + catch (Exception ex) + { + m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message); + return false; + } + } + + PrimMesher.SculptMesh.SculptType sculptType; + switch ((OpenMetaverse.SculptType)primShape.SculptType) + { + case OpenMetaverse.SculptType.Cylinder: + sculptType = PrimMesher.SculptMesh.SculptType.cylinder; + break; + case OpenMetaverse.SculptType.Plane: + sculptType = PrimMesher.SculptMesh.SculptType.plane; + break; + case OpenMetaverse.SculptType.Torus: + sculptType = PrimMesher.SculptMesh.SculptType.torus; + break; + case OpenMetaverse.SculptType.Sphere: + sculptType = PrimMesher.SculptMesh.SculptType.sphere; + break; + default: + sculptType = PrimMesher.SculptMesh.SculptType.plane; + break; + } + + bool mirror = ((primShape.SculptType & 128) != 0); + bool invert = ((primShape.SculptType & 64) != 0); + + sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, false, mirror, invert); + + idata.Dispose(); + + sculptMesh.DumpRaw(baseDir, primName, "primMesh"); + + sculptMesh.Scale(size.X, size.Y, size.Z); + + coords = sculptMesh.coords; + faces = sculptMesh.faces; + + return true; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the shape data the accompanies a prim. + /// + /// + /// + /// + /// Coords are added to this list by the method. + /// Faces are added to this list by the method. + /// true if coords and faces were successfully generated, false if not + private bool GenerateCoordsAndFacesFromPrimShapeData( + string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, out List coords, out List faces) + { + PrimMesh primMesh; + coords = new List(); + faces = new List(); + + float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f; + float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f; + float pathBegin = (float)primShape.PathBegin * 2.0e-5f; + float pathEnd = 1.0f - (float)primShape.PathEnd * 2.0e-5f; + float pathScaleX = (float)(primShape.PathScaleX - 100) * 0.01f; + float pathScaleY = (float)(primShape.PathScaleY - 100) * 0.01f; + + float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f; + float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f; + float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f; + if (profileHollow > 0.95f) + profileHollow = 0.95f; + + int sides = 4; + LevelOfDetail iLOD = (LevelOfDetail)lod; + if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + sides = 3; + else if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + switch (iLOD) + { + case LevelOfDetail.High: sides = 24; break; + case LevelOfDetail.Medium: sides = 12; break; + case LevelOfDetail.Low: sides = 6; break; + case LevelOfDetail.VeryLow: sides = 3; break; + default: sides = 24; break; + } + } + else if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { // half circle, prim is a sphere + switch (iLOD) + { + case LevelOfDetail.High: sides = 24; break; + case LevelOfDetail.Medium: sides = 12; break; + case LevelOfDetail.Low: sides = 6; break; + case LevelOfDetail.VeryLow: sides = 3; break; + default: sides = 24; break; + } + + profileBegin = 0.5f * profileBegin + 0.5f; + profileEnd = 0.5f * profileEnd + 0.5f; + } + + int hollowSides = sides; + if (primShape.HollowShape == HollowShape.Circle) + { + switch (iLOD) + { + case LevelOfDetail.High: hollowSides = 24; break; + case LevelOfDetail.Medium: hollowSides = 12; break; + case LevelOfDetail.Low: hollowSides = 6; break; + case LevelOfDetail.VeryLow: hollowSides = 3; break; + default: hollowSides = 24; break; + } + } + else if (primShape.HollowShape == HollowShape.Square) + hollowSides = 4; + else if (primShape.HollowShape == HollowShape.Triangle) + hollowSides = 3; + + primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides); + + if (primMesh.errorMessage != null) + if (primMesh.errorMessage.Length > 0) + m_log.Error("[ERROR] " + primMesh.errorMessage); + + primMesh.topShearX = pathShearX; + primMesh.topShearY = pathShearY; + primMesh.pathCutBegin = pathBegin; + primMesh.pathCutEnd = pathEnd; + + if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible) + { + primMesh.twistBegin = primShape.PathTwistBegin * 18 / 10; + primMesh.twistEnd = primShape.PathTwist * 18 / 10; + primMesh.taperX = pathScaleX; + primMesh.taperY = pathScaleY; + + if (profileBegin < 0.0f || profileBegin >= profileEnd || profileEnd > 1.0f) + { + ReportPrimError("*** CORRUPT PRIM!! ***", primName, primMesh); + if (profileBegin < 0.0f) profileBegin = 0.0f; + if (profileEnd > 1.0f) profileEnd = 1.0f; + } +#if SPAM + m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString()); +#endif + try + { + primMesh.ExtrudeLinear(); + } + catch (Exception ex) + { + ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh); + return false; + } + } + else + { + primMesh.holeSizeX = (200 - primShape.PathScaleX) * 0.01f; + primMesh.holeSizeY = (200 - primShape.PathScaleY) * 0.01f; + primMesh.radius = 0.01f * primShape.PathRadiusOffset; + primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions; + primMesh.skew = 0.01f * primShape.PathSkew; + primMesh.twistBegin = primShape.PathTwistBegin * 36 / 10; + primMesh.twistEnd = primShape.PathTwist * 36 / 10; + primMesh.taperX = primShape.PathTaperX * 0.01f; + primMesh.taperY = primShape.PathTaperY * 0.01f; + + if (profileBegin < 0.0f || profileBegin >= profileEnd || profileEnd > 1.0f) + { + ReportPrimError("*** CORRUPT PRIM!! ***", primName, primMesh); + if (profileBegin < 0.0f) profileBegin = 0.0f; + if (profileEnd > 1.0f) profileEnd = 1.0f; + } +#if SPAM + m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString()); +#endif + try + { + primMesh.ExtrudeCircular(); + } + catch (Exception ex) + { + ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh); + return false; + } + } + + primMesh.DumpRaw(baseDir, primName, "primMesh"); + + primMesh.Scale(size.X, size.Y, size.Z); + + coords = primMesh.coords; + faces = primMesh.faces; + + return true; + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) + { + return CreateMesh(primName, primShape, size, lod, false); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) + { +#if SPAM + m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName); +#endif + + Mesh mesh = null; + ulong key = 0; + + // If this mesh has been created already, return it instead of creating another copy + // For large regions with 100k+ prims and hundreds of copies of each, this can save a GB or more of memory + key = primShape.GetMeshKey(size, lod); + if (m_uniqueMeshes.TryGetValue(key, out mesh)) + return mesh; + + if (size.X < 0.01f) size.X = 0.01f; + if (size.Y < 0.01f) size.Y = 0.01f; + if (size.Z < 0.01f) size.Z = 0.01f; + + mesh = CreateMeshFromPrimMesher(primName, primShape, size, lod); + + if (mesh != null) + { + if ((!isPhysical) && size.X < minSizeForComplexMesh && size.Y < minSizeForComplexMesh && size.Z < minSizeForComplexMesh) + { +#if SPAM + m_log.Debug("Meshmerizer: prim " + primName + " has a size of " + size.ToString() + " which is below threshold of " + + minSizeForComplexMesh.ToString() + " - creating simple bounding box"); +#endif + mesh = CreateBoundingBoxMesh(mesh); + mesh.DumpRaw(baseDir, primName, "Z extruded"); + } + + // trim the vertex and triangle lists to free up memory + mesh.TrimExcess(); + + m_uniqueMeshes.Add(key, mesh); + } + + return mesh; + } + } +} diff --git a/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs new file mode 100644 index 0000000..53022ad --- /dev/null +++ b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs @@ -0,0 +1,2284 @@ +/* + * Copyright (c) Contributors + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Text; +using System.IO; + +namespace PrimMesher +{ + public struct Quat + { + /// X value + public float X; + /// Y value + public float Y; + /// Z value + public float Z; + /// W value + public float W; + + public Quat(float x, float y, float z, float w) + { + X = x; + Y = y; + Z = z; + W = w; + } + + public Quat(Coord axis, float angle) + { + axis = axis.Normalize(); + + angle *= 0.5f; + float c = (float)Math.Cos(angle); + float s = (float)Math.Sin(angle); + + X = axis.X * s; + Y = axis.Y * s; + Z = axis.Z * s; + W = c; + + Normalize(); + } + + public float Length() + { + return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W); + } + + public Quat Normalize() + { + const float MAG_THRESHOLD = 0.0000001f; + float mag = Length(); + + // Catch very small rounding errors when normalizing + if (mag > MAG_THRESHOLD) + { + float oomag = 1f / mag; + X *= oomag; + Y *= oomag; + Z *= oomag; + W *= oomag; + } + else + { + X = 0f; + Y = 0f; + Z = 0f; + W = 1f; + } + + return this; + } + + public static Quat operator *(Quat q1, Quat q2) + { + float x = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y; + float y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X; + float z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W; + float w = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z; + return new Quat(x, y, z, w); + } + + public override string ToString() + { + return "< X: " + this.X.ToString() + ", Y: " + this.Y.ToString() + ", Z: " + this.Z.ToString() + ", W: " + this.W.ToString() + ">"; + } + } + + public struct Coord + { + public float X; + public float Y; + public float Z; + + public Coord(float x, float y, float z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + + public float Length() + { + return (float)Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z); + } + + public Coord Invert() + { + this.X = -this.X; + this.Y = -this.Y; + this.Z = -this.Z; + + return this; + } + + public Coord Normalize() + { + const float MAG_THRESHOLD = 0.0000001f; + float mag = Length(); + + // Catch very small rounding errors when normalizing + if (mag > MAG_THRESHOLD) + { + float oomag = 1.0f / mag; + this.X *= oomag; + this.Y *= oomag; + this.Z *= oomag; + } + else + { + this.X = 0.0f; + this.Y = 0.0f; + this.Z = 0.0f; + } + + return this; + } + + public override string ToString() + { + return this.X.ToString() + " " + this.Y.ToString() + " " + this.Z.ToString(); + } + + public static Coord Cross(Coord c1, Coord c2) + { + return new Coord( + c1.Y * c2.Z - c2.Y * c1.Z, + c1.Z * c2.X - c2.Z * c1.X, + c1.X * c2.Y - c2.X * c1.Y + ); + } + + public static Coord operator +(Coord v, Coord a) + { + return new Coord(v.X + a.X, v.Y + a.Y, v.Z + a.Z); + } + + public static Coord operator *(Coord v, Coord m) + { + return new Coord(v.X * m.X, v.Y * m.Y, v.Z * m.Z); + } + + public static Coord operator *(Coord v, Quat q) + { + // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ + + Coord c2 = new Coord(0.0f, 0.0f, 0.0f); + + c2.X = q.W * q.W * v.X + + 2f * q.Y * q.W * v.Z - + 2f * q.Z * q.W * v.Y + + q.X * q.X * v.X + + 2f * q.Y * q.X * v.Y + + 2f * q.Z * q.X * v.Z - + q.Z * q.Z * v.X - + q.Y * q.Y * v.X; + + c2.Y = + 2f * q.X * q.Y * v.X + + q.Y * q.Y * v.Y + + 2f * q.Z * q.Y * v.Z + + 2f * q.W * q.Z * v.X - + q.Z * q.Z * v.Y + + q.W * q.W * v.Y - + 2f * q.X * q.W * v.Z - + q.X * q.X * v.Y; + + c2.Z = + 2f * q.X * q.Z * v.X + + 2f * q.Y * q.Z * v.Y + + q.Z * q.Z * v.Z - + 2f * q.W * q.Y * v.X - + q.Y * q.Y * v.Z + + 2f * q.W * q.X * v.Y - + q.X * q.X * v.Z + + q.W * q.W * v.Z; + + return c2; + } + } + + public struct UVCoord + { + public float U; + public float V; + + + public UVCoord(float u, float v) + { + this.U = u; + this.V = v; + } + } + + public struct Face + { + public int primFace; + + // vertices + public int v1; + public int v2; + public int v3; + + //normals + public int n1; + public int n2; + public int n3; + + // uvs + public int uv1; + public int uv2; + public int uv3; + + public Face(int v1, int v2, int v3) + { + primFace = 0; + + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + + this.n1 = 0; + this.n2 = 0; + this.n3 = 0; + + this.uv1 = 0; + this.uv2 = 0; + this.uv3 = 0; + + } + + public Face(int v1, int v2, int v3, int n1, int n2, int n3) + { + primFace = 0; + + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + + this.n1 = n1; + this.n2 = n2; + this.n3 = n3; + + this.uv1 = 0; + this.uv2 = 0; + this.uv3 = 0; + } + + public Coord SurfaceNormal(List coordList) + { + Coord c1 = coordList[this.v1]; + Coord c2 = coordList[this.v2]; + Coord c3 = coordList[this.v3]; + + Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z); + Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z); + + return Coord.Cross(edge1, edge2).Normalize(); + } + } + + public struct ViewerFace + { + public int primFaceNumber; + + public Coord v1; + public Coord v2; + public Coord v3; + + public int coordIndex1; + public int coordIndex2; + public int coordIndex3; + + public Coord n1; + public Coord n2; + public Coord n3; + + public UVCoord uv1; + public UVCoord uv2; + public UVCoord uv3; + + public ViewerFace(int primFaceNumber) + { + this.primFaceNumber = primFaceNumber; + + this.v1 = new Coord(); + this.v2 = new Coord(); + this.v3 = new Coord(); + + this.coordIndex1 = this.coordIndex2 = this.coordIndex3 = -1; // -1 means not assigned yet + + this.n1 = new Coord(); + this.n2 = new Coord(); + this.n3 = new Coord(); + + this.uv1 = new UVCoord(); + this.uv2 = new UVCoord(); + this.uv3 = new UVCoord(); + } + + public void Scale(float x, float y, float z) + { + this.v1.X *= x; + this.v1.Y *= y; + this.v1.Z *= z; + + this.v2.X *= x; + this.v2.Y *= y; + this.v2.Z *= z; + + this.v3.X *= x; + this.v3.Y *= y; + this.v3.Z *= z; + } + + public void AddPos(float x, float y, float z) + { + this.v1.X += x; + this.v2.X += x; + this.v3.X += x; + + this.v1.Y += y; + this.v2.Y += y; + this.v3.Y += y; + + this.v1.Z += z; + this.v2.Z += z; + this.v3.Z += z; + } + + public void AddRot(Quat q) + { + this.v1 *= q; + this.v2 *= q; + this.v3 *= q; + + this.n1 *= q; + this.n2 *= q; + this.n3 *= q; + } + + public void CalcSurfaceNormal() + { + + Coord edge1 = new Coord(this.v2.X - this.v1.X, this.v2.Y - this.v1.Y, this.v2.Z - this.v1.Z); + Coord edge2 = new Coord(this.v3.X - this.v1.X, this.v3.Y - this.v1.Y, this.v3.Z - this.v1.Z); + + this.n1 = this.n2 = this.n3 = Coord.Cross(edge1, edge2).Normalize(); + } + } + + internal struct Angle + { + internal float angle; + internal float X; + internal float Y; + + internal Angle(float angle, float x, float y) + { + this.angle = angle; + this.X = x; + this.Y = y; + } + } + + internal class AngleList + { + private float iX, iY; // intersection point + + private static Angle[] angles3 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Coord[] normals3 = + { + new Coord(0.25f, 0.4330127019f, 0.0f).Normalize(), + new Coord(-0.5f, 0.0f, 0.0f).Normalize(), + new Coord(0.25f, -0.4330127019f, 0.0f).Normalize(), + new Coord(0.25f, 0.4330127019f, 0.0f).Normalize() + }; + + private static Angle[] angles4 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Coord[] normals4 = + { + new Coord(0.5f, 0.5f, 0.0f).Normalize(), + new Coord(-0.5f, 0.5f, 0.0f).Normalize(), + new Coord(-0.5f, -0.5f, 0.0f).Normalize(), + new Coord(0.5f, -0.5f, 0.0f).Normalize(), + new Coord(0.5f, 0.5f, 0.0f).Normalize() + }; + + private static Angle[] angles24 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.041666666666666664f, 0.96592582628906831f, 0.25881904510252074f), + new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f), + new Angle(0.125f, 0.70710678118654757f, 0.70710678118654746f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.20833333333333331f, 0.25881904510252096f, 0.9659258262890682f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.29166666666666663f, -0.25881904510252063f, 0.96592582628906831f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.375f, -0.70710678118654746f, 0.70710678118654757f), + new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f), + new Angle(0.45833333333333331f, -0.9659258262890682f, 0.25881904510252102f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.54166666666666663f, -0.96592582628906842f, -0.25881904510252035f), + new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f), + new Angle(0.62499999999999989f, -0.70710678118654791f, -0.70710678118654713f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.70833333333333326f, -0.25881904510252152f, -0.96592582628906809f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(0.79166666666666663f, 0.2588190451025203f, -0.96592582628906842f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(0.875f, 0.70710678118654735f, -0.70710678118654768f), + new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f), + new Angle(0.95833333333333326f, 0.96592582628906809f, -0.25881904510252157f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private Angle interpolatePoints(float newPoint, Angle p1, Angle p2) + { + float m = (newPoint - p1.angle) / (p2.angle - p1.angle); + return new Angle(newPoint, p1.X + m * (p2.X - p1.X), p1.Y + m * (p2.Y - p1.Y)); + } + + private void intersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4) + { // ref: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/ + double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1); + double uaNumerator = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3); + + if (denom != 0.0) + { + double ua = uaNumerator / denom; + iX = (float)(x1 + ua * (x2 - x1)); + iY = (float)(y1 + ua * (y2 - y1)); + } + } + + internal List angles; + internal List normals; + + internal void makeAngles(int sides, float startAngle, float stopAngle) + { + angles = new List(); + normals = new List(); + + double twoPi = System.Math.PI * 2.0; + float twoPiInv = 1.0f / (float)twoPi; + + if (sides < 1) + throw new Exception("number of sides not greater than zero"); + if (stopAngle <= startAngle) + throw new Exception("stopAngle not greater than startAngle"); + + if ((sides == 3 || sides == 4 || sides == 24)) + { + startAngle *= twoPiInv; + stopAngle *= twoPiInv; + + Angle[] sourceAngles; + if (sides == 3) + sourceAngles = angles3; + else if (sides == 4) + sourceAngles = angles4; + else sourceAngles = angles24; + + int startAngleIndex = (int)(startAngle * sides); + int endAngleIndex = sourceAngles.Length - 1; + if (stopAngle < 1.0f) + endAngleIndex = (int)(stopAngle * sides) + 1; + if (endAngleIndex == startAngleIndex) + endAngleIndex++; + + for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++) + { + angles.Add(sourceAngles[angleIndex]); + if (sides == 3) + normals.Add(normals3[angleIndex]); + else if (sides == 4) + normals.Add(normals4[angleIndex]); + } + + if (startAngle > 0.0f) + angles[0] = interpolatePoints(startAngle, angles[0], angles[1]); + + if (stopAngle < 1.0f) + { + int lastAngleIndex = angles.Count - 1; + angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]); + } + } + else + { + double stepSize = twoPi / sides; + + int startStep = (int)(startAngle / stepSize); + double angle = stepSize * startStep; + int step = startStep; + double stopAngleTest = stopAngle; + if (stopAngle < twoPi) + { + stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1); + if (stopAngleTest < stopAngle) + stopAngleTest += stepSize; + if (stopAngleTest > twoPi) + stopAngleTest = twoPi; + } + + while (angle <= stopAngleTest) + { + Angle newAngle; + newAngle.angle = (float)angle; + newAngle.X = (float)System.Math.Cos(angle); + newAngle.Y = (float)System.Math.Sin(angle); + angles.Add(newAngle); + step += 1; + angle = stepSize * step; + } + + if (startAngle > angles[0].angle) + { + Angle newAngle; + intersection(angles[0].X, angles[0].Y, angles[1].X, angles[1].Y, 0.0f, 0.0f, (float)Math.Cos(startAngle), (float)Math.Sin(startAngle)); + newAngle.angle = startAngle; + newAngle.X = iX; + newAngle.Y = iY; + angles[0] = newAngle; + } + + int index = angles.Count - 1; + if (stopAngle < angles[index].angle) + { + Angle newAngle; + intersection(angles[index - 1].X, angles[index - 1].Y, angles[index].X, angles[index].Y, 0.0f, 0.0f, (float)Math.Cos(stopAngle), (float)Math.Sin(stopAngle)); + newAngle.angle = stopAngle; + newAngle.X = iX; + newAngle.Y = iY; + angles[index] = newAngle; + } + } + } + } + + /// + /// generates a profile for extrusion + /// + internal class Profile + { + private const float twoPi = 2.0f * (float)Math.PI; + + internal string errorMessage = null; + + internal List coords; + internal List faces; + internal List vertexNormals; + internal List us; + internal List faceUVs; + internal List faceNumbers; + + // use these for making individual meshes for each prim face + internal List outerCoordIndices = null; + internal List hollowCoordIndices = null; + internal List cut1CoordIndices = null; + internal List cut2CoordIndices = null; + + internal Coord faceNormal = new Coord(0.0f, 0.0f, 1.0f); + internal Coord cutNormal1 = new Coord(); + internal Coord cutNormal2 = new Coord(); + + internal int numOuterVerts = 0; + internal int numHollowVerts = 0; + + internal int outerFaceNumber = -1; + internal int hollowFaceNumber = -1; + + internal bool calcVertexNormals = false; + internal int bottomFaceNumber = 0; + internal int numPrimFaces = 0; + + internal Profile() + { + this.coords = new List(); + this.faces = new List(); + this.vertexNormals = new List(); + this.us = new List(); + this.faceUVs = new List(); + this.faceNumbers = new List(); + } + + internal Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool createFaces, bool calcVertexNormals) + { + this.calcVertexNormals = calcVertexNormals; + this.coords = new List(); + this.faces = new List(); + this.vertexNormals = new List(); + this.us = new List(); + this.faceUVs = new List(); + this.faceNumbers = new List(); + + Coord center = new Coord(0.0f, 0.0f, 0.0f); + //bool hasCenter = false; + + List hollowCoords = new List(); + List hollowNormals = new List(); + List hollowUs = new List(); + + if (calcVertexNormals) + { + this.outerCoordIndices = new List(); + this.hollowCoordIndices = new List(); + this.cut1CoordIndices = new List(); + this.cut2CoordIndices = new List(); + } + + bool hasHollow = (hollow > 0.0f); + + bool hasProfileCut = (profileStart > 0.0f || profileEnd < 1.0f); + + AngleList angles = new AngleList(); + AngleList hollowAngles = new AngleList(); + + float xScale = 0.5f; + float yScale = 0.5f; + if (sides == 4) // corners of a square are sqrt(2) from center + { + xScale = 0.707f; + yScale = 0.707f; + } + + float startAngle = profileStart * twoPi; + float stopAngle = profileEnd * twoPi; + + try { angles.makeAngles(sides, startAngle, stopAngle); } + catch (Exception ex) + { + + errorMessage = "makeAngles failed: Exception: " + ex.ToString() + + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString(); + + return; + } + + this.numOuterVerts = angles.angles.Count; + + // flag to create as few triangles as possible for 3 or 4 side profile + bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut); + + if (hasHollow) + { + if (sides == hollowSides) + hollowAngles = angles; + else + { + try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle); } + catch (Exception ex) + { + errorMessage = "makeAngles failed: Exception: " + ex.ToString() + + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString(); + + return; + } + } + this.numHollowVerts = hollowAngles.angles.Count; + } + else if (!simpleFace) + { + this.coords.Add(center); + //hasCenter = true; + if (this.calcVertexNormals) + this.vertexNormals.Add(new Coord(0.0f, 0.0f, 1.0f)); + this.us.Add(0.0f); + } + + float z = 0.0f; + + Angle angle; + Coord newVert = new Coord(); + if (hasHollow && hollowSides != sides) + { + int numHollowAngles = hollowAngles.angles.Count; + for (int i = 0; i < numHollowAngles; i++) + { + angle = hollowAngles.angles[i]; + newVert.X = hollow * xScale * angle.X; + newVert.Y = hollow * yScale * angle.Y; + newVert.Z = z; + + hollowCoords.Add(newVert); + if (this.calcVertexNormals) + { + if (hollowSides < 5) + hollowNormals.Add(hollowAngles.normals[i].Invert()); + else + hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f)); + + hollowUs.Add(angle.angle * hollow); + } + } + } + + int index = 0; + int numAngles = angles.angles.Count; + + for (int i = 0; i < numAngles; i++) + { + angle = angles.angles[i]; + newVert.X = angle.X * xScale; + newVert.Y = angle.Y * yScale; + newVert.Z = z; + this.coords.Add(newVert); + if (this.calcVertexNormals) + { + this.outerCoordIndices.Add(this.coords.Count - 1); + + if (sides < 5) + { + this.vertexNormals.Add(angles.normals[i]); + float u = angle.angle; + this.us.Add(u); + } + else + { + this.vertexNormals.Add(new Coord(angle.X, angle.Y, 0.0f)); + this.us.Add(angle.angle); + } + } + + if (hasHollow) + { + if (hollowSides == sides) + { + newVert.X *= hollow; + newVert.Y *= hollow; + newVert.Z = z; + hollowCoords.Add(newVert); + if (this.calcVertexNormals) + { + if (sides < 5) + { + hollowNormals.Add(angles.normals[i].Invert()); + } + + else + hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f)); + + hollowUs.Add(angle.angle * hollow); + } + } + } + else if (!simpleFace && createFaces && angle.angle > 0.0001f) + { + Face newFace = new Face(); + newFace.v1 = 0; + newFace.v2 = index; + newFace.v3 = index + 1; + + this.faces.Add(newFace); + } + index += 1; + } + + if (hasHollow) + { + hollowCoords.Reverse(); + if (this.calcVertexNormals) + { + hollowNormals.Reverse(); + hollowUs.Reverse(); + } + + if (createFaces) + { + //int numOuterVerts = this.coords.Count; + //numOuterVerts = this.coords.Count; + //int numHollowVerts = hollowCoords.Count; + int numTotalVerts = this.numOuterVerts + this.numHollowVerts; + + if (this.numOuterVerts == this.numHollowVerts) + { + Face newFace = new Face(); + + for (int coordIndex = 0; coordIndex < this.numOuterVerts - 1; coordIndex++) + { + newFace.v1 = coordIndex; + newFace.v2 = coordIndex + 1; + newFace.v3 = numTotalVerts - coordIndex - 1; + this.faces.Add(newFace); + + newFace.v1 = coordIndex + 1; + newFace.v2 = numTotalVerts - coordIndex - 2; + newFace.v3 = numTotalVerts - coordIndex - 1; + this.faces.Add(newFace); + } + } + else + { + if (this.numOuterVerts < this.numHollowVerts) + { + Face newFace = new Face(); + int j = 0; // j is the index for outer vertices + int maxJ = this.numOuterVerts - 1; + for (int i = 0; i < this.numHollowVerts; i++) // i is the index for inner vertices + { + if (j < maxJ) + if (angles.angles[j + 1].angle - hollowAngles.angles[i].angle < hollowAngles.angles[i].angle - angles.angles[j].angle + 0.000001f) + { + newFace.v1 = numTotalVerts - i - 1; + newFace.v2 = j; + newFace.v3 = j + 1; + + this.faces.Add(newFace); + j += 1; + } + + newFace.v1 = j; + newFace.v2 = numTotalVerts - i - 2; + newFace.v3 = numTotalVerts - i - 1; + + this.faces.Add(newFace); + } + } + else // numHollowVerts < numOuterVerts + { + Face newFace = new Face(); + int j = 0; // j is the index for inner vertices + int maxJ = this.numHollowVerts - 1; + for (int i = 0; i < this.numOuterVerts; i++) + { + if (j < maxJ) + if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f) + { + newFace.v1 = i; + newFace.v2 = numTotalVerts - j - 2; + newFace.v3 = numTotalVerts - j - 1; + + this.faces.Add(newFace); + j += 1; + } + + newFace.v1 = numTotalVerts - j - 1; + newFace.v2 = i; + newFace.v3 = i + 1; + + this.faces.Add(newFace); + } + } + } + } + + if (calcVertexNormals) + { + foreach (Coord hc in hollowCoords) + { + this.coords.Add(hc); + hollowCoordIndices.Add(this.coords.Count - 1); + } + } + else + this.coords.AddRange(hollowCoords); + + if (this.calcVertexNormals) + { + this.vertexNormals.AddRange(hollowNormals); + this.us.AddRange(hollowUs); + + } + } + + if (simpleFace && createFaces) + { + if (sides == 3) + this.faces.Add(new Face(0, 1, 2)); + else if (sides == 4) + { + this.faces.Add(new Face(0, 1, 2)); + this.faces.Add(new Face(0, 2, 3)); + } + } + + if (calcVertexNormals && hasProfileCut) + { + int lastOuterVertIndex = this.numOuterVerts - 1; + + if (hasHollow) + { + this.cut1CoordIndices.Add(0); + this.cut1CoordIndices.Add(this.coords.Count - 1); + + this.cut2CoordIndices.Add(lastOuterVertIndex + 1); + this.cut2CoordIndices.Add(lastOuterVertIndex); + + this.cutNormal1.X = this.coords[0].Y - this.coords[this.coords.Count - 1].Y; + this.cutNormal1.Y = -(this.coords[0].X - this.coords[this.coords.Count - 1].X); + + this.cutNormal2.X = this.coords[lastOuterVertIndex + 1].Y - this.coords[lastOuterVertIndex].Y; + this.cutNormal2.Y = -(this.coords[lastOuterVertIndex + 1].X - this.coords[lastOuterVertIndex].X); + } + + else + { + this.cut1CoordIndices.Add(0); + this.cut1CoordIndices.Add(1); + + this.cut2CoordIndices.Add(lastOuterVertIndex); + this.cut2CoordIndices.Add(0); + + this.cutNormal1.X = this.vertexNormals[1].Y; + this.cutNormal1.Y = -this.vertexNormals[1].X; + + this.cutNormal2.X = -this.vertexNormals[this.vertexNormals.Count - 2].Y; + this.cutNormal2.Y = this.vertexNormals[this.vertexNormals.Count - 2].X; + + } + this.cutNormal1.Normalize(); + this.cutNormal2.Normalize(); + } + + this.MakeFaceUVs(); + + hollowCoords = null; + hollowNormals = null; + hollowUs = null; + + if (calcVertexNormals) + { // calculate prim face numbers + + // face number order is top, outer, hollow, bottom, start cut, end cut + // I know it's ugly but so is the whole concept of prim face numbers + + int faceNum = 1; // start with outer faces + this.outerFaceNumber = faceNum; + + int startVert = hasProfileCut && !hasHollow ? 1 : 0; + if (startVert > 0) + this.faceNumbers.Add(-1); + for (int i = 0; i < this.numOuterVerts - 1; i++) + //this.faceNumbers.Add(sides < 5 ? faceNum++ : faceNum); + this.faceNumbers.Add(sides < 5 && i < sides ? faceNum++ : faceNum); + + //if (!hasHollow && !hasProfileCut) + // this.bottomFaceNumber = faceNum++; + + this.faceNumbers.Add(hasProfileCut ? -1 : faceNum++); + + if (sides > 4 && (hasHollow || hasProfileCut)) + faceNum++; + + if (sides < 5 && (hasHollow || hasProfileCut) && this.numOuterVerts < sides) + faceNum++; + + if (hasHollow) + { + for (int i = 0; i < this.numHollowVerts; i++) + this.faceNumbers.Add(faceNum); + + this.hollowFaceNumber = faceNum++; + } + //if (hasProfileCut || hasHollow) + // this.bottomFaceNumber = faceNum++; + this.bottomFaceNumber = faceNum++; + + if (hasHollow && hasProfileCut) + this.faceNumbers.Add(faceNum++); + + for (int i = 0; i < this.faceNumbers.Count; i++) + if (this.faceNumbers[i] == -1) + this.faceNumbers[i] = faceNum++; + + this.numPrimFaces = faceNum; + } + + } + + internal void MakeFaceUVs() + { + this.faceUVs = new List(); + foreach (Coord c in this.coords) + this.faceUVs.Add(new UVCoord(0.5f + c.X, 0.5f - c.Y)); + } + + internal Profile Copy() + { + return this.Copy(true); + } + + internal Profile Copy(bool needFaces) + { + Profile copy = new Profile(); + + copy.coords.AddRange(this.coords); + copy.faceUVs.AddRange(this.faceUVs); + + if (needFaces) + copy.faces.AddRange(this.faces); + if ((copy.calcVertexNormals = this.calcVertexNormals) == true) + { + copy.vertexNormals.AddRange(this.vertexNormals); + copy.faceNormal = this.faceNormal; + copy.cutNormal1 = this.cutNormal1; + copy.cutNormal2 = this.cutNormal2; + copy.us.AddRange(this.us); + copy.faceNumbers.AddRange(this.faceNumbers); + + copy.cut1CoordIndices = new List(this.cut1CoordIndices); + copy.cut2CoordIndices = new List(this.cut2CoordIndices); + copy.hollowCoordIndices = new List(this.hollowCoordIndices); + copy.outerCoordIndices = new List(this.outerCoordIndices); + } + copy.numOuterVerts = this.numOuterVerts; + copy.numHollowVerts = this.numHollowVerts; + + return copy; + } + + internal void AddPos(Coord v) + { + this.AddPos(v.X, v.Y, v.Z); + } + + internal void AddPos(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = this.coords[i]; + vert.X += x; + vert.Y += y; + vert.Z += z; + this.coords[i] = vert; + } + } + + internal void AddRot(Quat q) + { + int i; + int numVerts = this.coords.Count; + + for (i = 0; i < numVerts; i++) + this.coords[i] *= q; + + if (this.calcVertexNormals) + { + int numNormals = this.vertexNormals.Count; + for (i = 0; i < numNormals; i++) + this.vertexNormals[i] *= q; + + this.faceNormal *= q; + this.cutNormal1 *= q; + this.cutNormal2 *= q; + + } + } + + internal void Scale(float x, float y) + { + int i; + int numVerts = this.coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = this.coords[i]; + vert.X *= x; + vert.Y *= y; + this.coords[i] = vert; + } + } + + /// + /// Changes order of the vertex indices and negates the center vertex normal. Does not alter vertex normals of radial vertices + /// + internal void FlipNormals() + { + int i; + int numFaces = this.faces.Count; + Face tmpFace; + int tmp; + + for (i = 0; i < numFaces; i++) + { + tmpFace = this.faces[i]; + tmp = tmpFace.v3; + tmpFace.v3 = tmpFace.v1; + tmpFace.v1 = tmp; + this.faces[i] = tmpFace; + } + + if (this.calcVertexNormals) + { + int normalCount = this.vertexNormals.Count; + if (normalCount > 0) + { + Coord n = this.vertexNormals[normalCount - 1]; + n.Z = -n.Z; + this.vertexNormals[normalCount - 1] = n; + } + } + + this.faceNormal.X = -this.faceNormal.X; + this.faceNormal.Y = -this.faceNormal.Y; + this.faceNormal.Z = -this.faceNormal.Z; + + int numfaceUVs = this.faceUVs.Count; + for (i = 0; i < numfaceUVs; i++) + { + UVCoord uv = this.faceUVs[i]; + uv.V = 1.0f - uv.V; + this.faceUVs[i] = uv; + } + } + + internal void AddValue2FaceVertexIndices(int num) + { + int numFaces = this.faces.Count; + Face tmpFace; + for (int i = 0; i < numFaces; i++) + { + tmpFace = this.faces[i]; + tmpFace.v1 += num; + tmpFace.v2 += num; + tmpFace.v3 += num; + + this.faces[i] = tmpFace; + } + } + + internal void AddValue2FaceNormalIndices(int num) + { + if (this.calcVertexNormals) + { + int numFaces = this.faces.Count; + Face tmpFace; + for (int i = 0; i < numFaces; i++) + { + tmpFace = this.faces[i]; + tmpFace.n1 += num; + tmpFace.n2 += num; + tmpFace.n3 += num; + + this.faces[i] = tmpFace; + } + } + } + + internal void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < this.faces.Count; i++) + { + string s = this.coords[this.faces[i].v1].ToString(); + s += " " + this.coords[this.faces[i].v2].ToString(); + s += " " + this.coords[this.faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } + + public struct PathNode + { + public Coord position; + public Quat rotation; + public float xScale; + public float yScale; + public float percentOfPath; + } + + public enum PathType { Linear = 0, Circular = 1, Flexible = 2 } + + public class Path + { + public List pathNodes = new List(); + + public float twistBegin = 0.0f; + public float twistEnd = 0.0f; + public float topShearX = 0.0f; + public float topShearY = 0.0f; + public float pathCutBegin = 0.0f; + public float pathCutEnd = 1.0f; + public float dimpleBegin = 0.0f; + public float dimpleEnd = 1.0f; + public float skew = 0.0f; + public float holeSizeX = 1.0f; // called pathScaleX in pbs + public float holeSizeY = 0.25f; + public float taperX = 0.0f; + public float taperY = 0.0f; + public float radius = 0.0f; + public float revolutions = 1.0f; + public int stepsPerRevolution = 24; + + private const float twoPi = 2.0f * (float)Math.PI; + + public void Create(PathType pathType, int steps) + { + if (pathType == PathType.Linear || pathType == PathType.Flexible) + { + int step = 0; + + float length = this.pathCutEnd - this.pathCutBegin; + float twistTotal = twistEnd - twistBegin; + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number + + float start = -0.5f; + float stepSize = length / (float)steps; + float percentOfPathMultiplier = stepSize; + float xOffset = 0.0f; + float yOffset = 0.0f; + float zOffset = start; + float xOffsetStepIncrement = this.topShearX / steps; + float yOffsetStepIncrement = this.topShearY / steps; + + float percentOfPath = this.pathCutBegin; + zOffset += percentOfPath; + + // sanity checks + + bool done = false; + + while (!done) + { + PathNode newNode = new PathNode(); + + newNode.xScale = 1.0f; + if (this.taperX == 0.0f) + newNode.xScale = 1.0f; + else if (this.taperX > 0.0f) + newNode.xScale = 1.0f - percentOfPath * this.taperX; + else newNode.xScale = 1.0f + (1.0f - percentOfPath) * this.taperX; + + newNode.yScale = 1.0f; + if (this.taperY == 0.0f) + newNode.yScale = 1.0f; + else if (this.taperY > 0.0f) + newNode.yScale = 1.0f - percentOfPath * this.taperY; + else newNode.yScale = 1.0f + (1.0f - percentOfPath) * this.taperY; + + float twist = twistBegin + twistTotal * percentOfPath; + + newNode.rotation = new Quat(new Coord(0.0f, 0.0f, 1.0f), twist); + newNode.position = new Coord(xOffset, yOffset, zOffset); + newNode.percentOfPath = percentOfPath; + + pathNodes.Add(newNode); + + if (step < steps) + { + step += 1; + percentOfPath += percentOfPathMultiplier; + xOffset += xOffsetStepIncrement; + yOffset += yOffsetStepIncrement; + zOffset += stepSize; + if (percentOfPath > this.pathCutEnd) + done = true; + } + else done = true; + } + } // end of linear path code + + else // pathType == Circular + { + float twistTotal = twistEnd - twistBegin; + + // if the profile has a lot of twist, add more layers otherwise the layers may overlap + // and the resulting mesh may be quite inaccurate. This method is arbitrary and doesn't + // accurately match the viewer + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + { + if (twistTotalAbs > Math.PI * 1.5f) + steps *= 2; + if (twistTotalAbs > Math.PI * 3.0f) + steps *= 2; + } + + float yPathScale = this.holeSizeY * 0.5f; + float pathLength = this.pathCutEnd - this.pathCutBegin; + float totalSkew = this.skew * 2.0f * pathLength; + float skewStart = this.pathCutBegin * 2.0f * this.skew - this.skew; + float xOffsetTopShearXFactor = this.topShearX * (0.25f + 0.5f * (0.5f - this.holeSizeY)); + float yShearCompensation = 1.0f + Math.Abs(this.topShearY) * 0.25f; + + // It's not quite clear what pushY (Y top shear) does, but subtracting it from the start and end + // angles appears to approximate it's effects on path cut. Likewise, adding it to the angle used + // to calculate the sine for generating the path radius appears to approximate it's effects there + // too, but there are some subtle differences in the radius which are noticeable as the prim size + // increases and it may affect megaprims quite a bit. The effect of the Y top shear parameter on + // the meshes generated with this technique appear nearly identical in shape to the same prims when + // displayed by the viewer. + + float startAngle = (twoPi * this.pathCutBegin * this.revolutions) - this.topShearY * 0.9f; + float endAngle = (twoPi * this.pathCutEnd * this.revolutions) - this.topShearY * 0.9f; + float stepSize = twoPi / this.stepsPerRevolution; + + int step = (int)(startAngle / stepSize); + float angle = startAngle; + + bool done = false; + while (!done) // loop through the length of the path and add the layers + { + PathNode newNode = new PathNode(); + + float xProfileScale = (1.0f - Math.Abs(this.skew)) * this.holeSizeX; + float yProfileScale = this.holeSizeY; + + float percentOfPath = angle / (twoPi * this.revolutions); + float percentOfAngles = (angle - startAngle) / (endAngle - startAngle); + + if (this.taperX > 0.01f) + xProfileScale *= 1.0f - percentOfPath * this.taperX; + else if (this.taperX < -0.01f) + xProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperX; + + if (this.taperY > 0.01f) + yProfileScale *= 1.0f - percentOfPath * this.taperY; + else if (this.taperY < -0.01f) + yProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperY; + + newNode.xScale = xProfileScale; + newNode.yScale = yProfileScale; + + float radiusScale = 1.0f; + if (this.radius > 0.001f) + radiusScale = 1.0f - this.radius * percentOfPath; + else if (this.radius < 0.001f) + radiusScale = 1.0f + this.radius * (1.0f - percentOfPath); + + float twist = twistBegin + twistTotal * percentOfPath; + + float xOffset = 0.5f * (skewStart + totalSkew * percentOfAngles); + xOffset += (float)Math.Sin(angle) * xOffsetTopShearXFactor; + + float yOffset = yShearCompensation * (float)Math.Cos(angle) * (0.5f - yPathScale) * radiusScale; + + float zOffset = (float)Math.Sin(angle + this.topShearY) * (0.5f - yPathScale) * radiusScale; + + newNode.position = new Coord(xOffset, yOffset, zOffset); + + // now orient the rotation of the profile layer relative to it's position on the path + // adding taperY to the angle used to generate the quat appears to approximate the viewer + + newNode.rotation = new Quat(new Coord(1.0f, 0.0f, 0.0f), angle + this.topShearY); + + // next apply twist rotation to the profile layer + if (twistTotal != 0.0f || twistBegin != 0.0f) + newNode.rotation *= new Quat(new Coord(0.0f, 0.0f, 1.0f), twist); + + newNode.percentOfPath = percentOfPath; + + pathNodes.Add(newNode); + + // calculate terms for next iteration + // calculate the angle for the next iteration of the loop + + if (angle >= endAngle - 0.01) + done = true; + else + { + step += 1; + angle = stepSize * step; + if (angle > endAngle) + angle = endAngle; + } + } + } + } + } + + public class PrimMesh + { + public string errorMessage = ""; + private const float twoPi = 2.0f * (float)Math.PI; + + public List coords; + public List normals; + public List faces; + + public List viewerFaces; + + private int sides = 4; + private int hollowSides = 4; + private float profileStart = 0.0f; + private float profileEnd = 1.0f; + private float hollow = 0.0f; + public int twistBegin = 0; + public int twistEnd = 0; + public float topShearX = 0.0f; + public float topShearY = 0.0f; + public float pathCutBegin = 0.0f; + public float pathCutEnd = 1.0f; + public float dimpleBegin = 0.0f; + public float dimpleEnd = 1.0f; + public float skew = 0.0f; + public float holeSizeX = 1.0f; // called pathScaleX in pbs + public float holeSizeY = 0.25f; + public float taperX = 0.0f; + public float taperY = 0.0f; + public float radius = 0.0f; + public float revolutions = 1.0f; + public int stepsPerRevolution = 24; + + private int profileOuterFaceNumber = -1; + private int profileHollowFaceNumber = -1; + + private bool hasProfileCut = false; + private bool hasHollow = false; + public bool calcVertexNormals = false; + private bool normalsProcessed = false; + public bool viewerMode = false; + public bool sphereMode = false; + + public int numPrimFaces = 0; + + /// + /// Human readable string representation of the parameters used to create a mesh. + /// + /// + public string ParamsToDisplayString() + { + string s = ""; + s += "sides..................: " + this.sides.ToString(); + s += "\nhollowSides..........: " + this.hollowSides.ToString(); + s += "\nprofileStart.........: " + this.profileStart.ToString(); + s += "\nprofileEnd...........: " + this.profileEnd.ToString(); + s += "\nhollow...............: " + this.hollow.ToString(); + s += "\ntwistBegin...........: " + this.twistBegin.ToString(); + s += "\ntwistEnd.............: " + this.twistEnd.ToString(); + s += "\ntopShearX............: " + this.topShearX.ToString(); + s += "\ntopShearY............: " + this.topShearY.ToString(); + s += "\npathCutBegin.........: " + this.pathCutBegin.ToString(); + s += "\npathCutEnd...........: " + this.pathCutEnd.ToString(); + s += "\ndimpleBegin..........: " + this.dimpleBegin.ToString(); + s += "\ndimpleEnd............: " + this.dimpleEnd.ToString(); + s += "\nskew.................: " + this.skew.ToString(); + s += "\nholeSizeX............: " + this.holeSizeX.ToString(); + s += "\nholeSizeY............: " + this.holeSizeY.ToString(); + s += "\ntaperX...............: " + this.taperX.ToString(); + s += "\ntaperY...............: " + this.taperY.ToString(); + s += "\nradius...............: " + this.radius.ToString(); + s += "\nrevolutions..........: " + this.revolutions.ToString(); + s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString(); + s += "\nsphereMode...........: " + this.sphereMode.ToString(); + s += "\nhasProfileCut........: " + this.hasProfileCut.ToString(); + s += "\nhasHollow............: " + this.hasHollow.ToString(); + s += "\nviewerMode...........: " + this.viewerMode.ToString(); + + return s; + } + + public int ProfileOuterFaceNumber + { + get { return profileOuterFaceNumber; } + } + + public int ProfileHollowFaceNumber + { + get { return profileHollowFaceNumber; } + } + + public bool HasProfileCut + { + get { return hasProfileCut; } + } + + public bool HasHollow + { + get { return hasHollow; } + } + + + /// + /// Constructs a PrimMesh object and creates the profile for extrusion. + /// + /// + /// + /// + /// + /// + public PrimMesh(int sides, float profileStart, float profileEnd, float hollow, int hollowSides) + { + this.coords = new List(); + this.faces = new List(); + + this.sides = sides; + this.profileStart = profileStart; + this.profileEnd = profileEnd; + this.hollow = hollow; + this.hollowSides = hollowSides; + + if (sides < 3) + this.sides = 3; + if (hollowSides < 3) + this.hollowSides = 3; + if (profileStart < 0.0f) + this.profileStart = 0.0f; + if (profileEnd > 1.0f) + this.profileEnd = 1.0f; + if (profileEnd < 0.02f) + this.profileEnd = 0.02f; + if (profileStart >= profileEnd) + this.profileStart = profileEnd - 0.02f; + if (hollow > 0.99f) + this.hollow = 0.99f; + if (hollow < 0.0f) + this.hollow = 0.0f; + + //if (sphereMode) + // this.hasProfileCut = this.profileEnd - this.profileStart < 0.4999f; + //else + // //this.hasProfileCut = (this.profileStart > 0.0f || this.profileEnd < 1.0f); + // this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; + //this.hasHollow = (this.hollow > 0.001f); + } + + /// + /// Extrudes a profile along a path. + /// + public void Extrude(PathType pathType) + { + bool needEndFaces = false; + + this.coords = new List(); + this.faces = new List(); + + if (this.viewerMode) + { + this.viewerFaces = new List(); + this.calcVertexNormals = true; + } + + if (this.calcVertexNormals) + this.normals = new List(); + + int steps = 1; + + float length = this.pathCutEnd - this.pathCutBegin; + normalsProcessed = false; + + if (this.viewerMode && this.sides == 3) + { + // prisms don't taper well so add some vertical resolution + // other prims may benefit from this but just do prisms for now + if (Math.Abs(this.taperX) > 0.01 || Math.Abs(this.taperY) > 0.01) + steps = (int)(steps * 4.5 * length); + } + + if (sphereMode) + this.hasProfileCut = this.profileEnd - this.profileStart < 0.4999f; + else + //this.hasProfileCut = (this.profileStart > 0.0f || this.profileEnd < 1.0f); + this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; + this.hasHollow = (this.hollow > 0.001f); + + float twistBegin = this.twistBegin / 360.0f * twoPi; + float twistEnd = this.twistEnd / 360.0f * twoPi; + float twistTotal = twistEnd - twistBegin; + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number + + float hollow = this.hollow; + + // sanity checks + float initialProfileRot = 0.0f; + if (pathType == PathType.Circular) + { + if (this.sides == 3) + { + initialProfileRot = (float)Math.PI; + if (this.hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow *= 0.707f; + } + else hollow *= 0.5f; + } + else if (this.sides == 4) + { + initialProfileRot = 0.25f * (float)Math.PI; + if (this.hollowSides != 4) + hollow *= 0.707f; + } + else if (this.sides > 4) + { + initialProfileRot = (float)Math.PI; + if (this.hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow /= 0.7f; + } + } + } + else + { + if (this.sides == 3) + { + if (this.hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow *= 0.707f; + } + else hollow *= 0.5f; + } + else if (this.sides == 4) + { + initialProfileRot = 1.25f * (float)Math.PI; + if (this.hollowSides != 4) + hollow *= 0.707f; + } + else if (this.sides == 24 && this.hollowSides == 4) + hollow *= 1.414f; + } + + Profile profile = new Profile(this.sides, this.profileStart, this.profileEnd, hollow, this.hollowSides, true, calcVertexNormals); + this.errorMessage = profile.errorMessage; + + this.numPrimFaces = profile.numPrimFaces; + + //profileOuterFaceNumber = profile.faceNumbers[0]; + //if (!needEndFaces) + // profileOuterFaceNumber--; + //profileOuterFaceNumber = needEndFaces ? 1 : 0; + + + //if (hasHollow) + //{ + // if (needEndFaces) + // profileHollowFaceNumber = profile.faceNumbers[profile.numOuterVerts + 1]; + // else + // profileHollowFaceNumber = profile.faceNumbers[profile.numOuterVerts] - 1; + //} + + + profileOuterFaceNumber = profile.outerFaceNumber; + if (!needEndFaces) + profileOuterFaceNumber--; + + if (hasHollow) + { + profileHollowFaceNumber = profile.hollowFaceNumber; + if (!needEndFaces) + profileHollowFaceNumber--; + } + + int cut1Vert = -1; + int cut2Vert = -1; + if (hasProfileCut) + { + cut1Vert = hasHollow ? profile.coords.Count - 1 : 0; + cut2Vert = hasHollow ? profile.numOuterVerts - 1 : profile.numOuterVerts; + } + + if (initialProfileRot != 0.0f) + { + profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot)); + if (viewerMode) + profile.MakeFaceUVs(); + } + + Coord lastCutNormal1 = new Coord(); + Coord lastCutNormal2 = new Coord(); + float lastV = 1.0f; + + Path path = new Path(); + path.twistBegin = twistBegin; + path.twistEnd = twistEnd; + path.topShearX = topShearX; + path.topShearY = topShearY; + path.pathCutBegin = pathCutBegin; + path.pathCutEnd = pathCutEnd; + path.dimpleBegin = dimpleBegin; + path.dimpleEnd = dimpleEnd; + path.skew = skew; + path.holeSizeX = holeSizeX; + path.holeSizeY = holeSizeY; + path.taperX = taperX; + path.taperY = taperY; + path.radius = radius; + path.revolutions = revolutions; + path.stepsPerRevolution = stepsPerRevolution; + + path.Create(pathType, steps); + + + if (pathType == PathType.Circular) + { + needEndFaces = false; + if (this.pathCutBegin != 0.0f || this.pathCutEnd != 1.0f) + needEndFaces = true; + else if (this.taperX != 0.0f || this.taperY != 0.0f) + needEndFaces = true; + else if (this.skew != 0.0f) + needEndFaces = true; + else if (twistTotal != 0.0f) + needEndFaces = true; + else if (this.radius != 0.0f) + needEndFaces = true; + } + else needEndFaces = true; + + for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) + { + PathNode node = path.pathNodes[nodeIndex]; + Profile newLayer = profile.Copy(); + newLayer.Scale(node.xScale, node.yScale); + + newLayer.AddRot(node.rotation); + newLayer.AddPos(node.position); + + if (needEndFaces && nodeIndex == 0) + { + newLayer.FlipNormals(); + + // add the top faces to the viewerFaces list here + if (this.viewerMode) + { + Coord faceNormal = newLayer.faceNormal; + ViewerFace newViewerFace = new ViewerFace(profile.bottomFaceNumber); + int numFaces = newLayer.faces.Count; + List faces = newLayer.faces; + + for (int i = 0; i < numFaces; i++) + { + Face face = faces[i]; + newViewerFace.v1 = newLayer.coords[face.v1]; + newViewerFace.v2 = newLayer.coords[face.v2]; + newViewerFace.v3 = newLayer.coords[face.v3]; + + newViewerFace.coordIndex1 = face.v1; + newViewerFace.coordIndex2 = face.v2; + newViewerFace.coordIndex3 = face.v3; + + newViewerFace.n1 = faceNormal; + newViewerFace.n2 = faceNormal; + newViewerFace.n3 = faceNormal; + + newViewerFace.uv1 = newLayer.faceUVs[face.v1]; + newViewerFace.uv2 = newLayer.faceUVs[face.v2]; + newViewerFace.uv3 = newLayer.faceUVs[face.v3]; + + this.viewerFaces.Add(newViewerFace); + } + } + } // if (nodeIndex == 0) + + // append this layer + + int coordsLen = this.coords.Count; + newLayer.AddValue2FaceVertexIndices(coordsLen); + + this.coords.AddRange(newLayer.coords); + + if (this.calcVertexNormals) + { + newLayer.AddValue2FaceNormalIndices(this.normals.Count); + this.normals.AddRange(newLayer.vertexNormals); + } + + if (node.percentOfPath < this.pathCutBegin + 0.01f || node.percentOfPath > this.pathCutEnd - 0.01f) + this.faces.AddRange(newLayer.faces); + + // fill faces between layers + + int numVerts = newLayer.coords.Count; + Face newFace = new Face(); + + if (nodeIndex > 0) + { + int startVert = coordsLen + 1; + int endVert = this.coords.Count; + + if (sides < 5 || this.hasProfileCut || this.hasHollow) + startVert--; + + for (int i = startVert; i < endVert; i++) + { + int iNext = i + 1; + if (i == endVert - 1) + iNext = startVert; + + int whichVert = i - startVert; + + newFace.v1 = i; + newFace.v2 = i - numVerts; + newFace.v3 = iNext - numVerts; + this.faces.Add(newFace); + + newFace.v2 = iNext - numVerts; + newFace.v3 = iNext; + this.faces.Add(newFace); + + if (this.viewerMode) + { + // add the side faces to the list of viewerFaces here + + int primFaceNum = profile.faceNumbers[whichVert]; + if (!needEndFaces) + primFaceNum -= 1; + + ViewerFace newViewerFace1 = new ViewerFace(primFaceNum); + ViewerFace newViewerFace2 = new ViewerFace(primFaceNum); + + float u1 = newLayer.us[whichVert]; + float u2 = 1.0f; + if (whichVert < newLayer.us.Count - 1) + u2 = newLayer.us[whichVert + 1]; + + if (whichVert == cut1Vert || whichVert == cut2Vert) + { + u1 = 0.0f; + u2 = 1.0f; + } + else if (sides < 5) + { + if (whichVert < profile.numOuterVerts) + { // boxes and prisms have one texture face per side of the prim, so the U values have to be scaled + // to reflect the entire texture width + u1 *= sides; + u2 *= sides; + u2 -= (int)u1; + u1 -= (int)u1; + if (u2 < 0.1f) + u2 = 1.0f; + //this.profileOuterFaceNumber = primFaceNum; + } + else if (whichVert > profile.coords.Count - profile.numHollowVerts - 1) + { + u1 *= 2.0f; + u2 *= 2.0f; + //this.profileHollowFaceNumber = primFaceNum; + } + } + + newViewerFace1.uv1.U = u1; + newViewerFace1.uv2.U = u1; + newViewerFace1.uv3.U = u2; + + newViewerFace1.uv1.V = 1.0f - node.percentOfPath; + newViewerFace1.uv2.V = lastV; + newViewerFace1.uv3.V = lastV; + + newViewerFace2.uv1.U = u1; + newViewerFace2.uv2.U = u2; + newViewerFace2.uv3.U = u2; + + newViewerFace2.uv1.V = 1.0f - node.percentOfPath; + newViewerFace2.uv2.V = lastV; + newViewerFace2.uv3.V = 1.0f - node.percentOfPath; + + newViewerFace1.v1 = this.coords[i]; + newViewerFace1.v2 = this.coords[i - numVerts]; + newViewerFace1.v3 = this.coords[iNext - numVerts]; + + newViewerFace2.v1 = this.coords[i]; + newViewerFace2.v2 = this.coords[iNext - numVerts]; + newViewerFace2.v3 = this.coords[iNext]; + + newViewerFace1.coordIndex1 = i; + newViewerFace1.coordIndex2 = i - numVerts; + newViewerFace1.coordIndex3 = iNext - numVerts; + + newViewerFace2.coordIndex1 = i; + newViewerFace2.coordIndex2 = iNext - numVerts; + newViewerFace2.coordIndex3 = iNext; + + // profile cut faces + if (whichVert == cut1Vert) + { + newViewerFace1.n1 = newLayer.cutNormal1; + newViewerFace1.n2 = newViewerFace1.n3 = lastCutNormal1; + + newViewerFace2.n1 = newViewerFace2.n3 = newLayer.cutNormal1; + newViewerFace2.n2 = lastCutNormal1; + } + else if (whichVert == cut2Vert) + { + newViewerFace1.n1 = newLayer.cutNormal2; + newViewerFace1.n2 = newViewerFace1.n3 = lastCutNormal2; + + newViewerFace2.n1 = newViewerFace2.n3 = newLayer.cutNormal2; + newViewerFace2.n2 = lastCutNormal2; + } + + else // outer and hollow faces + { + if ((sides < 5 && whichVert < newLayer.numOuterVerts) || (hollowSides < 5 && whichVert >= newLayer.numOuterVerts)) + { // looks terrible when path is twisted... need vertex normals here + newViewerFace1.CalcSurfaceNormal(); + newViewerFace2.CalcSurfaceNormal(); + } + else + { + newViewerFace1.n1 = this.normals[i]; + newViewerFace1.n2 = this.normals[i - numVerts]; + newViewerFace1.n3 = this.normals[iNext - numVerts]; + + newViewerFace2.n1 = this.normals[i]; + newViewerFace2.n2 = this.normals[iNext - numVerts]; + newViewerFace2.n3 = this.normals[iNext]; + } + } + + this.viewerFaces.Add(newViewerFace1); + this.viewerFaces.Add(newViewerFace2); + + } + } + } + + lastCutNormal1 = newLayer.cutNormal1; + lastCutNormal2 = newLayer.cutNormal2; + lastV = 1.0f - node.percentOfPath; + + if (needEndFaces && nodeIndex == path.pathNodes.Count - 1 && viewerMode) + { + // add the top faces to the viewerFaces list here + Coord faceNormal = newLayer.faceNormal; + ViewerFace newViewerFace = new ViewerFace(); + newViewerFace.primFaceNumber = 0; + int numFaces = newLayer.faces.Count; + List faces = newLayer.faces; + + for (int i = 0; i < numFaces; i++) + { + Face face = faces[i]; + newViewerFace.v1 = newLayer.coords[face.v1 - coordsLen]; + newViewerFace.v2 = newLayer.coords[face.v2 - coordsLen]; + newViewerFace.v3 = newLayer.coords[face.v3 - coordsLen]; + + newViewerFace.coordIndex1 = face.v1 - coordsLen; + newViewerFace.coordIndex2 = face.v2 - coordsLen; + newViewerFace.coordIndex3 = face.v3 - coordsLen; + + newViewerFace.n1 = faceNormal; + newViewerFace.n2 = faceNormal; + newViewerFace.n3 = faceNormal; + + newViewerFace.uv1 = newLayer.faceUVs[face.v1 - coordsLen]; + newViewerFace.uv2 = newLayer.faceUVs[face.v2 - coordsLen]; + newViewerFace.uv3 = newLayer.faceUVs[face.v3 - coordsLen]; + + this.viewerFaces.Add(newViewerFace); + } + } + + + } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) + + } + + + /// + /// DEPRICATED - use Extrude(PathType.Linear) instead + /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism. + /// + /// + public void ExtrudeLinear() + { + this.Extrude(PathType.Linear); + } + + + /// + /// DEPRICATED - use Extrude(PathType.Circular) instead + /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring. + /// + /// + public void ExtrudeCircular() + { + this.Extrude(PathType.Circular); + } + + + private Coord SurfaceNormal(Coord c1, Coord c2, Coord c3) + { + Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z); + Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z); + + Coord normal = Coord.Cross(edge1, edge2); + + normal.Normalize(); + + return normal; + } + + private Coord SurfaceNormal(Face face) + { + return SurfaceNormal(this.coords[face.v1], this.coords[face.v2], this.coords[face.v3]); + } + + /// + /// Calculate the surface normal for a face in the list of faces + /// + /// + /// + public Coord SurfaceNormal(int faceIndex) + { + int numFaces = this.faces.Count; + if (faceIndex < 0 || faceIndex >= numFaces) + throw new Exception("faceIndex out of range"); + + return SurfaceNormal(this.faces[faceIndex]); + } + + /// + /// Duplicates a PrimMesh object. All object properties are copied by value, including lists. + /// + /// + public PrimMesh Copy() + { + PrimMesh copy = new PrimMesh(this.sides, this.profileStart, this.profileEnd, this.hollow, this.hollowSides); + copy.twistBegin = this.twistBegin; + copy.twistEnd = this.twistEnd; + copy.topShearX = this.topShearX; + copy.topShearY = this.topShearY; + copy.pathCutBegin = this.pathCutBegin; + copy.pathCutEnd = this.pathCutEnd; + copy.dimpleBegin = this.dimpleBegin; + copy.dimpleEnd = this.dimpleEnd; + copy.skew = this.skew; + copy.holeSizeX = this.holeSizeX; + copy.holeSizeY = this.holeSizeY; + copy.taperX = this.taperX; + copy.taperY = this.taperY; + copy.radius = this.radius; + copy.revolutions = this.revolutions; + copy.stepsPerRevolution = this.stepsPerRevolution; + copy.calcVertexNormals = this.calcVertexNormals; + copy.normalsProcessed = this.normalsProcessed; + copy.viewerMode = this.viewerMode; + copy.numPrimFaces = this.numPrimFaces; + copy.errorMessage = this.errorMessage; + + copy.coords = new List(this.coords); + copy.faces = new List(this.faces); + copy.viewerFaces = new List(this.viewerFaces); + copy.normals = new List(this.normals); + + return copy; + } + + /// + /// Calculate surface normals for all of the faces in the list of faces in this mesh + /// + public void CalcNormals() + { + if (normalsProcessed) + return; + + normalsProcessed = true; + + int numFaces = faces.Count; + + if (!this.calcVertexNormals) + this.normals = new List(); + + for (int i = 0; i < numFaces; i++) + { + Face face = faces[i]; + + this.normals.Add(SurfaceNormal(i).Normalize()); + + int normIndex = normals.Count - 1; + face.n1 = normIndex; + face.n2 = normIndex; + face.n3 = normIndex; + + this.faces[i] = face; + } + } + + /// + /// Adds a value to each XYZ vertex coordinate in the mesh + /// + /// + /// + /// + public void AddPos(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = this.coords[i]; + vert.X += x; + vert.Y += y; + vert.Z += z; + this.coords[i] = vert; + } + + if (this.viewerFaces != null) + { + int numViewerFaces = this.viewerFaces.Count; + + for (i = 0; i < numViewerFaces; i++) + { + ViewerFace v = this.viewerFaces[i]; + v.AddPos(x, y, z); + this.viewerFaces[i] = v; + } + } + } + + /// + /// Rotates the mesh + /// + /// + public void AddRot(Quat q) + { + int i; + int numVerts = this.coords.Count; + + for (i = 0; i < numVerts; i++) + this.coords[i] *= q; + + if (this.normals != null) + { + int numNormals = this.normals.Count; + for (i = 0; i < numNormals; i++) + this.normals[i] *= q; + } + + if (this.viewerFaces != null) + { + int numViewerFaces = this.viewerFaces.Count; + + for (i = 0; i < numViewerFaces; i++) + { + ViewerFace v = this.viewerFaces[i]; + v.v1 *= q; + v.v2 *= q; + v.v3 *= q; + + v.n1 *= q; + v.n2 *= q; + v.n3 *= q; + this.viewerFaces[i] = v; + } + } + } + +#if VERTEX_INDEXER + public VertexIndexer GetVertexIndexer() + { + if (this.viewerMode && this.viewerFaces.Count > 0) + return new VertexIndexer(this); + return null; + } +#endif + + /// + /// Scales the mesh + /// + /// + /// + /// + public void Scale(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + //Coord vert; + + Coord m = new Coord(x, y, z); + for (i = 0; i < numVerts; i++) + this.coords[i] *= m; + + if (this.viewerFaces != null) + { + int numViewerFaces = this.viewerFaces.Count; + for (i = 0; i < numViewerFaces; i++) + { + ViewerFace v = this.viewerFaces[i]; + v.v1 *= m; + v.v2 *= m; + v.v3 *= m; + this.viewerFaces[i] = v; + } + + } + + } + + /// + /// Dumps the mesh to a Blender compatible "Raw" format file + /// + /// + /// + /// + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < this.faces.Count; i++) + { + string s = this.coords[this.faces[i].v1].ToString(); + s += " " + this.coords[this.faces[i].v2].ToString(); + s += " " + this.coords[this.faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } +} diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs new file mode 100644 index 0000000..b3d9cb6 --- /dev/null +++ b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs @@ -0,0 +1,197 @@ +/* + * Copyright (c) Contributors + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// to build without references to System.Drawing, comment this out +#define SYSTEM_DRAWING + +using System; +using System.Collections.Generic; +using System.Text; + +#if SYSTEM_DRAWING +using System.Drawing; +using System.Drawing.Imaging; + +namespace PrimMesher +{ + public class SculptMap + { + public int width; + public int height; + public byte[] redBytes; + public byte[] greenBytes; + public byte[] blueBytes; + + public SculptMap() + { + } + + public SculptMap(Bitmap bm, int lod) + { + int bmW = bm.Width; + int bmH = bm.Height; + + if (bmW == 0 || bmH == 0) + throw new Exception("SculptMap: bitmap has no data"); + + int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + + bool smallMap = bmW * bmH <= numLodPixels; + bool needsScaling = false; + + width = bmW; + height = bmH; + while (width * height > numLodPixels * 4) + { + width >>= 1; + height >>= 1; + needsScaling = true; + } + + try + { + if (needsScaling) + bm = ScaleImage(bm, width, height); + } + + catch (Exception e) + { + throw new Exception("Exception in ScaleImage(): e: " + e.ToString()); + } + + if (width * height > numLodPixels) + { + width >>= 1; + height >>= 1; + } + + int numBytes = (width + 1) * (height + 1); + redBytes = new byte[numBytes]; + greenBytes = new byte[numBytes]; + blueBytes = new byte[numBytes]; + + int byteNdx = 0; + + try + { + for (int y = 0; y <= height; y++) + { + for (int x = 0; x <= width; x++) + { + Color c; + + if (smallMap) + c = bm.GetPixel(x < width ? x : x - 1, + y < height ? y : y - 1); + else + c = bm.GetPixel(x < width ? x * 2 : x * 2 - 1, + y < height ? y * 2 : y * 2 - 1); + + redBytes[byteNdx] = c.R; + greenBytes[byteNdx] = c.G; + blueBytes[byteNdx] = c.B; + + ++byteNdx; + } + } + } + catch (Exception e) + { + throw new Exception("Caught exception processing byte arrays in SculptMap(): e: " + e.ToString()); + } + + width++; + height++; + } + + public List> ToRows(bool mirror) + { + int numRows = height; + int numCols = width; + + List> rows = new List>(numRows); + + float pixScale = 1.0f / 255; + + int rowNdx, colNdx; + int smNdx = 0; + + + for (rowNdx = 0; rowNdx < numRows; rowNdx++) + { + List row = new List(numCols); + for (colNdx = 0; colNdx < numCols; colNdx++) + { + + if (mirror) + row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f)); + else + row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f)); + + ++smNdx; + } + rows.Add(row); + } + return rows; + } + + private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight) + { + + Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); + + Color c; + float xscale = srcImage.Width / destWidth; + float yscale = srcImage.Height / destHeight; + + float sy = 0.5f; + for (int y = 0; y < destHeight; y++) + { + float sx = 0.5f; + for (int x = 0; x < destWidth; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), (int)(sy)); + scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + + sx += xscale; + } + sy += yscale; + } + srcImage.Dispose(); + return scaledImage; + } + + } + + } +#endif diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs new file mode 100644 index 0000000..4a7f3ad --- /dev/null +++ b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs @@ -0,0 +1,646 @@ +/* + * Copyright (c) Contributors + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// to build without references to System.Drawing, comment this out +#define SYSTEM_DRAWING + +using System; +using System.Collections.Generic; +using System.Text; +using System.IO; + +#if SYSTEM_DRAWING +using System.Drawing; +using System.Drawing.Imaging; +#endif + +namespace PrimMesher +{ + + public class SculptMesh + { + public List coords; + public List faces; + + public List viewerFaces; + public List normals; + public List uvs; + + public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 }; + +#if SYSTEM_DRAWING + + public SculptMesh SculptMeshFromFile(string fileName, SculptType sculptType, int lod, bool viewerMode) + { + Bitmap bitmap = (Bitmap)Bitmap.FromFile(fileName); + SculptMesh sculptMesh = new SculptMesh(bitmap, sculptType, lod, viewerMode); + bitmap.Dispose(); + return sculptMesh; + } + + + public SculptMesh(string fileName, int sculptType, int lod, int viewerMode, int mirror, int invert) + { + Bitmap bitmap = (Bitmap)Bitmap.FromFile(fileName); + _SculptMesh(bitmap, (SculptType)sculptType, lod, viewerMode != 0, mirror != 0, invert != 0); + bitmap.Dispose(); + } +#endif + + /// + /// ** Experimental ** May disappear from future versions ** not recommeneded for use in applications + /// Construct a sculpt mesh from a 2D array of floats + /// + /// + /// + /// + /// + /// + /// + public SculptMesh(float[,] zMap, float xBegin, float xEnd, float yBegin, float yEnd, bool viewerMode) + { + float xStep, yStep; + float uStep, vStep; + + int numYElements = zMap.GetLength(0); + int numXElements = zMap.GetLength(1); + + try + { + xStep = (xEnd - xBegin) / (float)(numXElements - 1); + yStep = (yEnd - yBegin) / (float)(numYElements - 1); + + uStep = 1.0f / (numXElements - 1); + vStep = 1.0f / (numYElements - 1); + } + catch (DivideByZeroException) + { + return; + } + + coords = new List(); + faces = new List(); + normals = new List(); + uvs = new List(); + + viewerFaces = new List(); + + int p1, p2, p3, p4; + + int x, y; + int xStart = 0, yStart = 0; + + for (y = yStart; y < numYElements; y++) + { + int rowOffset = y * numXElements; + + for (x = xStart; x < numXElements; x++) + { + /* + * p1-----p2 + * | \ f2 | + * | \ | + * | f1 \| + * p3-----p4 + */ + + p4 = rowOffset + x; + p3 = p4 - 1; + + p2 = p4 - numXElements; + p1 = p3 - numXElements; + + Coord c = new Coord(xBegin + x * xStep, yBegin + y * yStep, zMap[y, x]); + this.coords.Add(c); + if (viewerMode) + { + this.normals.Add(new Coord()); + this.uvs.Add(new UVCoord(uStep * x, 1.0f - vStep * y)); + } + + if (y > 0 && x > 0) + { + Face f1, f2; + + if (viewerMode) + { + f1 = new Face(p1, p4, p3, p1, p4, p3); + f1.uv1 = p1; + f1.uv2 = p4; + f1.uv3 = p3; + + f2 = new Face(p1, p2, p4, p1, p2, p4); + f2.uv1 = p1; + f2.uv2 = p2; + f2.uv3 = p4; + } + else + { + f1 = new Face(p1, p4, p3); + f2 = new Face(p1, p2, p4); + } + + this.faces.Add(f1); + this.faces.Add(f2); + } + } + } + + if (viewerMode) + calcVertexNormals(SculptType.plane, numXElements, numYElements); + } + +#if SYSTEM_DRAWING + public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode) + { + _SculptMesh(sculptBitmap, sculptType, lod, viewerMode, false, false); + } + + public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode, bool mirror, bool invert) + { + _SculptMesh(sculptBitmap, sculptType, lod, viewerMode, mirror, invert); + } +#endif + + public SculptMesh(List> rows, SculptType sculptType, bool viewerMode, bool mirror, bool invert) + { + _SculptMesh(rows, sculptType, viewerMode, mirror, invert); + } + +#if SYSTEM_DRAWING + /// + /// converts a bitmap to a list of lists of coords, while scaling the image. + /// the scaling is done in floating point so as to allow for reduced vertex position + /// quantization as the position will be averaged between pixel values. this routine will + /// likely fail if the bitmap width and height are not powers of 2. + /// + /// + /// + /// + /// + private List> bitmap2Coords(Bitmap bitmap, int scale, bool mirror) + { + int numRows = bitmap.Height / scale; + int numCols = bitmap.Width / scale; + List> rows = new List>(numRows); + + float pixScale = 1.0f / (scale * scale); + pixScale /= 255; + + int imageX, imageY = 0; + + int rowNdx, colNdx; + + for (rowNdx = 0; rowNdx < numRows; rowNdx++) + { + List row = new List(numCols); + for (colNdx = 0; colNdx < numCols; colNdx++) + { + imageX = colNdx * scale; + int imageYStart = rowNdx * scale; + int imageYEnd = imageYStart + scale; + int imageXEnd = imageX + scale; + float rSum = 0.0f; + float gSum = 0.0f; + float bSum = 0.0f; + for (; imageX < imageXEnd; imageX++) + { + for (imageY = imageYStart; imageY < imageYEnd; imageY++) + { + Color c = bitmap.GetPixel(imageX, imageY); + if (c.A != 255) + { + bitmap.SetPixel(imageX, imageY, Color.FromArgb(255, c.R, c.G, c.B)); + c = bitmap.GetPixel(imageX, imageY); + } + rSum += c.R; + gSum += c.G; + bSum += c.B; + } + } + if (mirror) + row.Add(new Coord(-(rSum * pixScale - 0.5f), gSum * pixScale - 0.5f, bSum * pixScale - 0.5f)); + else + row.Add(new Coord(rSum * pixScale - 0.5f, gSum * pixScale - 0.5f, bSum * pixScale - 0.5f)); + + } + rows.Add(row); + } + return rows; + } + + private List> bitmap2CoordsSampled(Bitmap bitmap, int scale, bool mirror) + { + int numRows = bitmap.Height / scale; + int numCols = bitmap.Width / scale; + List> rows = new List>(numRows); + + float pixScale = 1.0f / 256.0f; + + int imageX, imageY = 0; + + int rowNdx, colNdx; + + for (rowNdx = 0; rowNdx <= numRows; rowNdx++) + { + List row = new List(numCols); + imageY = rowNdx * scale; + if (rowNdx == numRows) imageY--; + for (colNdx = 0; colNdx <= numCols; colNdx++) + { + imageX = colNdx * scale; + if (colNdx == numCols) imageX--; + + Color c = bitmap.GetPixel(imageX, imageY); + if (c.A != 255) + { + bitmap.SetPixel(imageX, imageY, Color.FromArgb(255, c.R, c.G, c.B)); + c = bitmap.GetPixel(imageX, imageY); + } + + if (mirror) + row.Add(new Coord(-(c.R * pixScale - 0.5f), c.G * pixScale - 0.5f, c.B * pixScale - 0.5f)); + else + row.Add(new Coord(c.R * pixScale - 0.5f, c.G * pixScale - 0.5f, c.B * pixScale - 0.5f)); + + } + rows.Add(row); + } + return rows; + } + + + void _SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode, bool mirror, bool invert) + { + _SculptMesh(new SculptMap(sculptBitmap, lod).ToRows(mirror), sculptType, viewerMode, mirror, invert); + } +#endif + + void _SculptMesh(List> rows, SculptType sculptType, bool viewerMode, bool mirror, bool invert) + { + coords = new List(); + faces = new List(); + normals = new List(); + uvs = new List(); + + sculptType = (SculptType)(((int)sculptType) & 0x07); + + if (mirror) + invert = !invert; + + viewerFaces = new List(); + + int width = rows[0].Count; + + int p1, p2, p3, p4; + + int imageX, imageY; + + if (sculptType != SculptType.plane) + { + if (rows.Count % 2 == 0) + { + for (int rowNdx = 0; rowNdx < rows.Count; rowNdx++) + rows[rowNdx].Add(rows[rowNdx][0]); + } + else + { + int lastIndex = rows[0].Count - 1; + + for (int i = 0; i < rows.Count; i++) + rows[i][0] = rows[i][lastIndex]; + } + } + + Coord topPole = rows[0][width / 2]; + Coord bottomPole = rows[rows.Count - 1][width / 2]; + + if (sculptType == SculptType.sphere) + { + if (rows.Count % 2 == 0) + { + int count = rows[0].Count; + List topPoleRow = new List(count); + List bottomPoleRow = new List(count); + + for (int i = 0; i < count; i++) + { + topPoleRow.Add(topPole); + bottomPoleRow.Add(bottomPole); + } + rows.Insert(0, topPoleRow); + rows.Add(bottomPoleRow); + } + else + { + int count = rows[0].Count; + + List topPoleRow = rows[0]; + List bottomPoleRow = rows[rows.Count - 1]; + + for (int i = 0; i < count; i++) + { + topPoleRow[i] = topPole; + bottomPoleRow[i] = bottomPole; + } + } + } + + if (sculptType == SculptType.torus) + rows.Add(rows[0]); + + int coordsDown = rows.Count; + int coordsAcross = rows[0].Count; +// int lastColumn = coordsAcross - 1; + + float widthUnit = 1.0f / (coordsAcross - 1); + float heightUnit = 1.0f / (coordsDown - 1); + + for (imageY = 0; imageY < coordsDown; imageY++) + { + int rowOffset = imageY * coordsAcross; + + for (imageX = 0; imageX < coordsAcross; imageX++) + { + /* + * p1-----p2 + * | \ f2 | + * | \ | + * | f1 \| + * p3-----p4 + */ + + p4 = rowOffset + imageX; + p3 = p4 - 1; + + p2 = p4 - coordsAcross; + p1 = p3 - coordsAcross; + + this.coords.Add(rows[imageY][imageX]); + if (viewerMode) + { + this.normals.Add(new Coord()); + this.uvs.Add(new UVCoord(widthUnit * imageX, heightUnit * imageY)); + } + + if (imageY > 0 && imageX > 0) + { + Face f1, f2; + + if (viewerMode) + { + if (invert) + { + f1 = new Face(p1, p4, p3, p1, p4, p3); + f1.uv1 = p1; + f1.uv2 = p4; + f1.uv3 = p3; + + f2 = new Face(p1, p2, p4, p1, p2, p4); + f2.uv1 = p1; + f2.uv2 = p2; + f2.uv3 = p4; + } + else + { + f1 = new Face(p1, p3, p4, p1, p3, p4); + f1.uv1 = p1; + f1.uv2 = p3; + f1.uv3 = p4; + + f2 = new Face(p1, p4, p2, p1, p4, p2); + f2.uv1 = p1; + f2.uv2 = p4; + f2.uv3 = p2; + } + } + else + { + if (invert) + { + f1 = new Face(p1, p4, p3); + f2 = new Face(p1, p2, p4); + } + else + { + f1 = new Face(p1, p3, p4); + f2 = new Face(p1, p4, p2); + } + } + + this.faces.Add(f1); + this.faces.Add(f2); + } + } + } + + if (viewerMode) + calcVertexNormals(sculptType, coordsAcross, coordsDown); + } + + /// + /// Duplicates a SculptMesh object. All object properties are copied by value, including lists. + /// + /// + public SculptMesh Copy() + { + return new SculptMesh(this); + } + + public SculptMesh(SculptMesh sm) + { + coords = new List(sm.coords); + faces = new List(sm.faces); + viewerFaces = new List(sm.viewerFaces); + normals = new List(sm.normals); + uvs = new List(sm.uvs); + } + + private void calcVertexNormals(SculptType sculptType, int xSize, int ySize) + { // compute vertex normals by summing all the surface normals of all the triangles sharing + // each vertex and then normalizing + int numFaces = this.faces.Count; + for (int i = 0; i < numFaces; i++) + { + Face face = this.faces[i]; + Coord surfaceNormal = face.SurfaceNormal(this.coords); + this.normals[face.n1] += surfaceNormal; + this.normals[face.n2] += surfaceNormal; + this.normals[face.n3] += surfaceNormal; + } + + int numNormals = this.normals.Count; + for (int i = 0; i < numNormals; i++) + this.normals[i] = this.normals[i].Normalize(); + + if (sculptType != SculptType.plane) + { // blend the vertex normals at the cylinder seam + for (int y = 0; y < ySize; y++) + { + int rowOffset = y * xSize; + + this.normals[rowOffset] = this.normals[rowOffset + xSize - 1] = (this.normals[rowOffset] + this.normals[rowOffset + xSize - 1]).Normalize(); + } + } + + foreach (Face face in this.faces) + { + ViewerFace vf = new ViewerFace(0); + vf.v1 = this.coords[face.v1]; + vf.v2 = this.coords[face.v2]; + vf.v3 = this.coords[face.v3]; + + vf.coordIndex1 = face.v1; + vf.coordIndex2 = face.v2; + vf.coordIndex3 = face.v3; + + vf.n1 = this.normals[face.n1]; + vf.n2 = this.normals[face.n2]; + vf.n3 = this.normals[face.n3]; + + vf.uv1 = this.uvs[face.uv1]; + vf.uv2 = this.uvs[face.uv2]; + vf.uv3 = this.uvs[face.uv3]; + + this.viewerFaces.Add(vf); + } + } + + /// + /// Adds a value to each XYZ vertex coordinate in the mesh + /// + /// + /// + /// + public void AddPos(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = this.coords[i]; + vert.X += x; + vert.Y += y; + vert.Z += z; + this.coords[i] = vert; + } + + if (this.viewerFaces != null) + { + int numViewerFaces = this.viewerFaces.Count; + + for (i = 0; i < numViewerFaces; i++) + { + ViewerFace v = this.viewerFaces[i]; + v.AddPos(x, y, z); + this.viewerFaces[i] = v; + } + } + } + + /// + /// Rotates the mesh + /// + /// + public void AddRot(Quat q) + { + int i; + int numVerts = this.coords.Count; + + for (i = 0; i < numVerts; i++) + this.coords[i] *= q; + + int numNormals = this.normals.Count; + for (i = 0; i < numNormals; i++) + this.normals[i] *= q; + + if (this.viewerFaces != null) + { + int numViewerFaces = this.viewerFaces.Count; + + for (i = 0; i < numViewerFaces; i++) + { + ViewerFace v = this.viewerFaces[i]; + v.v1 *= q; + v.v2 *= q; + v.v3 *= q; + + v.n1 *= q; + v.n2 *= q; + v.n3 *= q; + + this.viewerFaces[i] = v; + } + } + } + + public void Scale(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + + Coord m = new Coord(x, y, z); + for (i = 0; i < numVerts; i++) + this.coords[i] *= m; + + if (this.viewerFaces != null) + { + int numViewerFaces = this.viewerFaces.Count; + for (i = 0; i < numViewerFaces; i++) + { + ViewerFace v = this.viewerFaces[i]; + v.v1 *= m; + v.v2 *= m; + v.v3 *= m; + this.viewerFaces[i] = v; + } + } + } + + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < this.faces.Count; i++) + { + string s = this.coords[this.faces[i].v1].ToString(); + s += " " + this.coords[this.faces[i].v2].ToString(); + s += " " + this.coords[this.faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } +} -- cgit v1.1 From 8c1550b58e70b98bacd5fe9ecf710b59a4c43198 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 20 Mar 2012 19:24:45 +0000 Subject: WORK in progress!! Now it reads the simple hull shape to use if convex shape is selected for a prim. Due to ODE limitations on convex hulls colisions, it creates a mesh. Being work in progress it is hardcoded to only read that simple convex hull for now. It writes a file named "lixo_lixo.raw" that can be imported into blender for examination of the created mesh (the last one loaded and also hardcoded). To play with put in opensim.ini "meshing = UbitMeshmerizer" --- OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs | 108 +------------ OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 180 +++++++++++++++++++--- 2 files changed, 164 insertions(+), 124 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs b/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs index 8cd8dcf..2938257 100644 --- a/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs +++ b/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs @@ -270,116 +270,20 @@ public class Triangle public Vertex v2; public Vertex v3; - private float radius_square; - private float cx; - private float cy; - public Triangle(Vertex _v1, Vertex _v2, Vertex _v3) { v1 = _v1; v2 = _v2; v3 = _v3; - - CalcCircle(); - } - - public bool isInCircle(float x, float y) - { - float dx, dy; - float dd; - - dx = x - cx; - dy = y - cy; - - dd = dx*dx + dy*dy; - if (dd < radius_square) - return true; - else - return false; } - public bool isDegraded() + public Triangle(float _v1x,float _v1y,float _v1z, + float _v2x,float _v2y,float _v2z, + float _v3x,float _v3y,float _v3z) { - // This means, the vertices of this triangle are somewhat strange. - // They either line up or at least two of them are identical - return (radius_square == 0.0); - } - - private void CalcCircle() - { - // Calculate the center and the radius of a circle given by three points p1, p2, p3 - // It is assumed, that the triangles vertices are already set correctly - double p1x, p2x, p1y, p2y, p3x, p3y; - - // Deviation of this routine: - // A circle has the general equation (M-p)^2=r^2, where M and p are vectors - // this gives us three equations f(p)=r^2, each for one point p1, p2, p3 - // putting respectively two equations together gives two equations - // f(p1)=f(p2) and f(p1)=f(p3) - // bringing all constant terms to one side brings them to the form - // M*v1=c1 resp.M*v2=c2 where v1=(p1-p2) and v2=(p1-p3) (still vectors) - // and c1, c2 are scalars (Naming conventions like the variables below) - // Now using the equations that are formed by the components of the vectors - // and isolate Mx lets you make one equation that only holds My - // The rest is straight forward and eaasy :-) - // - - /* helping variables for temporary results */ - double c1, c2; - double v1x, v1y, v2x, v2y; - - double z, n; - - double rx, ry; - - // Readout the three points, the triangle consists of - p1x = v1.X; - p1y = v1.Y; - - p2x = v2.X; - p2y = v2.Y; - - p3x = v3.X; - p3y = v3.Y; - - /* calc helping values first */ - c1 = (p1x*p1x + p1y*p1y - p2x*p2x - p2y*p2y)/2; - c2 = (p1x*p1x + p1y*p1y - p3x*p3x - p3y*p3y)/2; - - v1x = p1x - p2x; - v1y = p1y - p2y; - - v2x = p1x - p3x; - v2y = p1y - p3y; - - z = (c1*v2x - c2*v1x); - n = (v1y*v2x - v2y*v1x); - - if (n == 0.0) // This is no triangle, i.e there are (at least) two points at the same location - { - radius_square = 0.0f; - return; - } - - cy = (float) (z/n); - - if (v2x != 0.0) - { - cx = (float) ((c2 - v2y*cy)/v2x); - } - else if (v1x != 0.0) - { - cx = (float) ((c1 - v1y*cy)/v1x); - } - else - { - Debug.Assert(false, "Malformed triangle"); /* Both terms zero means nothing good */ - } - - rx = (p1x - cx); - ry = (p1y - cy); - - radius_square = (float) (rx*rx + ry*ry); + v1 = new Vertex(_v1x, _v1y, _v1z); + v2 = new Vertex(_v2x, _v2y, _v2z); + v3 = new Vertex(_v3x, _v3y, _v3z); } public override String ToString() diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index c9c52c0..a04df81 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -41,6 +41,7 @@ using Nini.Config; using System.Reflection; using System.IO; using ComponentAce.Compression.Libs.zlib; +using OpenSim.Region.Physics.ConvexDecompositionDotNet; namespace OpenSim.Region.Physics.Meshing { @@ -296,22 +297,20 @@ namespace OpenSim.Region.Physics.Meshing int numCoords = coords.Count; int numFaces = faces.Count; - // Create the list of vertices - List vertices = new List(); - for (int i = 0; i < numCoords; i++) - { - Coord c = coords[i]; - vertices.Add(new Vertex(c.X, c.Y, c.Z)); - } - Mesh mesh = new Mesh(); // Add the corresponding triangles to the mesh for (int i = 0; i < numFaces; i++) { Face f = faces[i]; - mesh.Add(new Triangle(vertices[f.v1], vertices[f.v2], vertices[f.v3])); + mesh.Add(new Triangle(coords[f.v1].X, coords[f.v1].Y, coords[f.v1].Z, + coords[f.v2].X, coords[f.v2].Y, coords[f.v2].Z, + coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z)); } + + // mesh.DumpRaw("c:\\lixo", "lixo", "lixo"); + mesh.DumpRaw(".", "lixo", "lixo"); + return mesh; } @@ -329,6 +328,10 @@ namespace OpenSim.Region.Physics.Meshing { // m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); + + bool convex = true; // this will be a input + bool usemesh = false; + coords = new List(); faces = new List(); OSD meshOsd = null; @@ -365,14 +368,25 @@ namespace OpenSim.Region.Physics.Meshing { OSDMap physicsParms = null; OSDMap map = (OSDMap)meshOsd; - if (map.ContainsKey("physics_shape")) - physicsParms = (OSDMap)map["physics_shape"]; // old asset format - else if (map.ContainsKey("physics_mesh")) - physicsParms = (OSDMap)map["physics_mesh"]; // new asset format + + if (!convex) + { + if (map.ContainsKey("physics_shape")) + physicsParms = (OSDMap)map["physics_shape"]; // old asset format + else if (map.ContainsKey("physics_mesh")) + physicsParms = (OSDMap)map["physics_mesh"]; // new asset format + + if (physicsParms != null) + usemesh = true; + } + + if(!usemesh && (map.ContainsKey("physics_convex"))) + physicsParms = (OSDMap)map["physics_convex"]; + if (physicsParms == null) { - m_log.Warn("[MESH]: no recognized physics mesh found in mesh asset"); + m_log.Warn("[MESH]: unknown mesh type"); return false; } @@ -416,20 +430,142 @@ namespace OpenSim.Region.Physics.Meshing return false; } - OSDArray decodedMeshOsdArray = null; - // physics_shape is an array of OSDMaps, one for each submesh - if (decodedMeshOsd is OSDArray) + if (usemesh) { -// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); + OSDArray decodedMeshOsdArray = null; - decodedMeshOsdArray = (OSDArray)decodedMeshOsd; - foreach (OSD subMeshOsd in decodedMeshOsdArray) + // physics_shape is an array of OSDMaps, one for each submesh + if (decodedMeshOsd is OSDArray) { - if (subMeshOsd is OSDMap) - AddSubMesh(subMeshOsd as OSDMap, size, coords, faces); + // Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); + + decodedMeshOsdArray = (OSDArray)decodedMeshOsd; + foreach (OSD subMeshOsd in decodedMeshOsdArray) + { + if (subMeshOsd is OSDMap) + AddSubMesh(subMeshOsd as OSDMap, size, coords, faces); + } } } + else + { + OSDMap cmap = (OSDMap)decodedMeshOsd; + if (cmap == null) + return false; + + byte[] data; + const float invMaxU16 = 1.0f / 65535f; + int t1; + int t2; + int t3; + int i; + + List vs = new List(); + + float3 f3; + PHullResult hullr = new PHullResult(); + Mesh m = new Mesh(); + + Vector3 range; + Vector3 min; + + if (cmap.ContainsKey("Max")) + range = cmap["Max"].AsVector3(); + else + range = new Vector3(0.5f, 0.5f, 0.5f); + + if (cmap.ContainsKey("Min")) + min = cmap["Min"].AsVector3(); + else + min = new Vector3(-0.5f, -0.5f, -0.5f); + + range = range - min; + range *= invMaxU16; +/* + // if (!convex && cmap.ContainsKey("HullList")) + if (cmap.ContainsKey("HullList")) + { + List hsizes = new List(); + + data = cmap["HullList"].AsBinary(); + for (i = 0; i < data.Length; i++) + { + t1 = data[i]; + if (t1 == 0) + t1 = 256; + hsizes.Add(t1); + } + +bla bla + + + + } + */ + + if (cmap.ContainsKey("BoundingVerts")) + { + data = cmap["BoundingVerts"].AsBinary(); + + for (i = 0; i < data.Length; ) + { + t1 = data[i++]; + t1 += data[i++] << 8; + t2 = data[i++]; + t2 += data[i++] << 8; + t3 = data[i++]; + t3 += data[i++] << 8; + + f3 = new float3((t1 * range.X + min.X) * size.X, + (t2 * range.Y + min.Y) * size.Y, + (t3 * range.Z + min.Z) * size.Z); + vs.Add(f3); + } + + + if (!HullUtils.ComputeHull(vs, ref hullr, 300, 0.0f)) + return false; + + int nverts = hullr.Vertices.Count; + int nindexs = hullr.Indices.Count; + + if (nindexs % 3 != 0) + return false; + + Coord c; + for (i = 0; i < nverts; i++) + { + c.X = hullr.Vertices[i].x; + c.Y = hullr.Vertices[i].y; + c.Z = hullr.Vertices[i].z; + coords.Add(c); + } + + Face f; + + for (i = 0; i < nindexs; i += 3) + { + t1 = hullr.Indices[i]; + if (t1 > nverts) + break; + t2 = hullr.Indices[i + 1]; + if (t2 > nverts) + break; + t3 = hullr.Indices[i + 2]; + if (t3 > nverts) + break; + f = new Face(t1, t2, t3); + faces.Add(f); + } + + if (coords.Count > 0 && faces.Count > 0) + return true; + } + else + return false; + + } } return true; -- cgit v1.1 From 23e6a31aa50a331fdb7ba3bf23134c3808c80372 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 20 Mar 2012 23:38:04 +0000 Subject: added convex decomposition hulls support. Hardcoded to use mesh or this. (so no simple hull of convex prims for now). --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 164 +++++++++++++++++++--- 1 file changed, 146 insertions(+), 18 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index a04df81..a550342 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -308,7 +308,7 @@ namespace OpenSim.Region.Physics.Meshing } - // mesh.DumpRaw("c:\\lixo", "lixo", "lixo"); +// mesh.DumpRaw("c:\\lixo", "lixo", "lixo"); mesh.DumpRaw(".", "lixo", "lixo"); return mesh; @@ -329,7 +329,7 @@ namespace OpenSim.Region.Physics.Meshing // m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); - bool convex = true; // this will be a input + bool convex = false; // this will be a input bool usemesh = false; coords = new List(); @@ -430,7 +430,6 @@ namespace OpenSim.Region.Physics.Meshing return false; } - if (usemesh) { OSDArray decodedMeshOsdArray = null; @@ -465,10 +464,14 @@ namespace OpenSim.Region.Physics.Meshing float3 f3; PHullResult hullr = new PHullResult(); - Mesh m = new Mesh(); - + + Coord c; + Face f; + Vector3 range; Vector3 min; + int nverts; + int nindexs; if (cmap.ContainsKey("Max")) range = cmap["Max"].AsVector3(); @@ -482,27 +485,125 @@ namespace OpenSim.Region.Physics.Meshing range = range - min; range *= invMaxU16; -/* - // if (!convex && cmap.ContainsKey("HullList")) - if (cmap.ContainsKey("HullList")) + + if (!convex && cmap.ContainsKey("HullList") && cmap.ContainsKey("Positions")) { List hsizes = new List(); - + int totalpoints = 0; data = cmap["HullList"].AsBinary(); for (i = 0; i < data.Length; i++) { t1 = data[i]; if (t1 == 0) t1 = 256; + totalpoints += t1; hsizes.Add(t1); } -bla bla + data = cmap["Positions"].AsBinary(); + int ptr = 0; + int vertsoffset = 0; + + if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point + { + foreach (int hullsize in hsizes) + { + for (i = 0; i < hullsize; i++ ) + { + t1 = data[ptr++]; + t1 += data[ptr++] << 8; + t2 = data[ptr++]; + t2 += data[ptr++] << 8; + t3 = data[ptr++]; + t3 += data[ptr++] << 8; + + f3 = new float3((t1 * range.X + min.X) * size.X, + (t2 * range.Y + min.Y) * size.Y, + (t3 * range.Z + min.Z) * size.Z); + vs.Add(f3); + } + + if(hullsize <3) + { + vs.Clear(); + continue; + } + + if (hullsize <5) + { + foreach (float3 point in vs) + { + c.X = point.x; + c.Y = point.y; + c.Z = point.z; + coords.Add(c); + } + f = new Face(vertsoffset, vertsoffset + 1, vertsoffset + 2); + faces.Add(f); + + if (hullsize == 4) + { + // not sure about orientation.. + f = new Face(vertsoffset, vertsoffset + 2, vertsoffset + 3); + faces.Add(f); + f = new Face(vertsoffset, vertsoffset + 3, vertsoffset + 1); + faces.Add(f); + f = new Face(vertsoffset + 3, vertsoffset + 2, vertsoffset + 1); + faces.Add(f); + } + vertsoffset += vs.Count; + vs.Clear(); + continue; + } + + if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f)) + { + vs.Clear(); + continue; + } + + nverts = hullr.Vertices.Count; + nindexs = hullr.Indices.Count; + if (nindexs % 3 != 0) + { + vs.Clear(); + continue; + } + for (i = 0; i < nverts; i++) + { + c.X = hullr.Vertices[i].x; + c.Y = hullr.Vertices[i].y; + c.Z = hullr.Vertices[i].z; + coords.Add(c); + } + + for (i = 0; i < nindexs; i += 3) + { + t1 = hullr.Indices[i]; + if (t1 > nverts) + break; + t2 = hullr.Indices[i + 1]; + if (t2 > nverts) + break; + t3 = hullr.Indices[i + 2]; + if (t3 > nverts) + break; + f = new Face(vertsoffset + t1, vertsoffset + t2, vertsoffset + t3); + faces.Add(f); + } + vertsoffset += nverts; + vs.Clear(); + } + } + if (coords.Count > 0 && faces.Count > 0) + return true; + } - */ + + vs.Clear(); if (cmap.ContainsKey("BoundingVerts")) { @@ -523,17 +624,47 @@ bla bla vs.Add(f3); } + if (vs.Count < 3) + { + vs.Clear(); + return false; + } + + if (vs.Count < 5) + { + foreach (float3 point in vs) + { + c.X = point.x; + c.Y = point.y; + c.Z = point.z; + coords.Add(c); + } + f = new Face(0, 1, 2); + faces.Add(f); + + if (vs.Count == 4) + { + // not sure about orientation.. + f = new Face(0, 2, 3); + faces.Add(f); + f = new Face(0, 3, 1); + faces.Add(f); + f = new Face( 3, 2, 1); + faces.Add(f); + } + vs.Clear(); + return true; + } - if (!HullUtils.ComputeHull(vs, ref hullr, 300, 0.0f)) + if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f)) return false; - int nverts = hullr.Vertices.Count; - int nindexs = hullr.Indices.Count; + nverts = hullr.Vertices.Count; + nindexs = hullr.Indices.Count; if (nindexs % 3 != 0) return false; - Coord c; for (i = 0; i < nverts; i++) { c.X = hullr.Vertices[i].x; @@ -541,9 +672,6 @@ bla bla c.Z = hullr.Vertices[i].z; coords.Add(c); } - - Face f; - for (i = 0; i < nindexs; i += 3) { t1 = hullr.Indices[i]; -- cgit v1.1 From 8817b6e74cd792808c4bf1d7c125ff3ecace4d89 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 21 Mar 2012 00:02:41 +0000 Subject: let convex be a parameter to createMesh so it can be used. Was forced to add it also to original mesher code and zeromesher --- OpenSim/Region/Physics/Manager/IMesher.cs | 1 + OpenSim/Region/Physics/Manager/ZeroMesher.cs | 5 +++++ OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 5 +++++ OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 17 ++++++++++------- 4 files changed, 21 insertions(+), 7 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index cc92484..c32cf38 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -36,6 +36,7 @@ namespace OpenSim.Region.Physics.Manager { IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod); IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical); + IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical,bool convex); } // Values for level of detail to be passed to the mesher. diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs index ba19db6..8a3b50b 100644 --- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs +++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs @@ -67,6 +67,11 @@ namespace OpenSim.Region.Physics.Manager return CreateMesh(primName, primShape, size, lod, false); } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + return CreateMesh(primName, primShape, size, lod, false); + } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { // Remove the reference to the encoded JPEG2000 data so it can be GCed diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index c4b245f..5597542 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -717,6 +717,11 @@ namespace OpenSim.Region.Physics.Meshing return CreateMesh(primName, primShape, size, lod, false); } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + return CreateMesh(primName, primShape, size, lod, false); + } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { #if SPAM diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index a550342..7667e91 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -257,7 +257,7 @@ namespace OpenSim.Region.Physics.Meshing /// /// /// - private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, Vector3 size, float lod) + private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool convex) { // m_log.DebugFormat( // "[MESH]: Creating physics proxy for {0}, shape {1}", @@ -273,7 +273,7 @@ namespace OpenSim.Region.Physics.Meshing if (!useMeshiesPhysicsMesh) return null; - if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, size, out coords, out faces)) + if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, size, out coords, out faces, convex)) return null; } else @@ -324,12 +324,10 @@ namespace OpenSim.Region.Physics.Meshing /// Faces are added to this list by the method. /// true if coords and faces were successfully generated, false if not private bool GenerateCoordsAndFacesFromPrimMeshData( - string primName, PrimitiveBaseShape primShape, Vector3 size, out List coords, out List faces) + string primName, PrimitiveBaseShape primShape, Vector3 size, out List coords, out List faces, bool convex) { // m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); - - bool convex = false; // this will be a input bool usemesh = false; coords = new List(); @@ -978,11 +976,16 @@ namespace OpenSim.Region.Physics.Meshing public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) { - return CreateMesh(primName, primShape, size, lod, false); + return CreateMesh(primName, primShape, size, lod, false,false); } public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { + return CreateMesh(primName, primShape, size, lod, false,false); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { #if SPAM m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName); #endif @@ -1000,7 +1003,7 @@ namespace OpenSim.Region.Physics.Meshing if (size.Y < 0.01f) size.Y = 0.01f; if (size.Z < 0.01f) size.Z = 0.01f; - mesh = CreateMeshFromPrimMesher(primName, primShape, size, lod); + mesh = CreateMeshFromPrimMesher(primName, primShape, size, lod,convex); if (mesh != null) { -- cgit v1.1 From 2e41294da969cd124a013070aaf6ce5dd9f01a5c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 21 Mar 2012 01:24:30 +0000 Subject: add convex state to mesh key, so a change is detected. --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 7667e91..df08381 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -995,7 +995,7 @@ namespace OpenSim.Region.Physics.Meshing // If this mesh has been created already, return it instead of creating another copy // For large regions with 100k+ prims and hundreds of copies of each, this can save a GB or more of memory - key = primShape.GetMeshKey(size, lod); + key = primShape.GetMeshKey(size, lod, convex); if (m_uniqueMeshes.TryGetValue(key, out mesh)) return mesh; -- cgit v1.1 From 11ed932263161d1dbea99d4a5699ba6d00894053 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 21 Mar 2012 01:46:41 +0000 Subject: Tell physics about physics shape when creating. Added some virtual methods to get/set density,gravmod, frition,bounce and shape type ( not in use ). UbitOde now should do convex type on creation or everytime the mesh is changed ( as in change size, shape, etc ) --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 7 ++++++ OpenSim/Region/Physics/Manager/PhysicsScene.cs | 7 ++++++ OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 14 ++++++++--- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 32 ++++++++++++++++++++++-- 4 files changed, 55 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index bd80fff..be67204 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -172,6 +172,8 @@ namespace OpenSim.Region.Physics.Manager public virtual bool Phantom { get; set; } + public virtual byte PhysicsShapeType { get; set; } + public abstract PrimitiveBaseShape Shape { set; } uint m_baseLocalID; @@ -252,6 +254,11 @@ namespace OpenSim.Region.Physics.Manager { } + public virtual float Density { get; set; } + public virtual float GravModifier { get; set; } + public virtual float Friction { get; set; } + public virtual float Bounce { get; set; } + /// /// Position of this actor. /// diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index eca6a0f..f2c0c28 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -137,6 +137,13 @@ namespace OpenSim.Region.Physics.Manager return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid); } + + public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid) + { + return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid); + } + public virtual float TimeDilation { get { return 1.0f; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 39b89d3..fd2f88f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -163,7 +163,7 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr collide_geom; // for objects: geom if single prim space it linkset private float m_density = 10.000006836f; // Aluminum g/cm3; - + private byte m_shapetype; public bool _zeroFlag; private bool m_lastUpdateSent; @@ -846,7 +846,7 @@ namespace OpenSim.Region.Physics.OdePlugin public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,uint plocalID) + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) { Name = primName; LocalID = plocalID; @@ -920,6 +920,8 @@ namespace OpenSim.Region.Physics.OdePlugin hasOOBoffsetFromMesh = false; _triMeshData = IntPtr.Zero; + m_shapetype = _shapeType; + m_lastdoneSelected = false; m_isSelected = false; m_delaySelect = false; @@ -1050,7 +1052,13 @@ namespace OpenSim.Region.Physics.OdePlugin } } - IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true); + bool convex; + if (m_shapetype == 0) + convex = false; + else + convex = true; + + IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex); if (mesh == null) { m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 612eafd..76d7746 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1141,7 +1141,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,localID); + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,0,localID); lock (_prims) _prims.Add(newPrim); @@ -1159,7 +1159,25 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,isPhantom,localID); + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, 0, localID); + + lock (_prims) + _prims.Add(newPrim); + } + return newPrim; + } + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) + { + Vector3 pos = position; + Vector3 siz = size; + Quaternion rot = rotation; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, shapeType, localID); lock (_prims) _prims.Add(newPrim); @@ -1203,6 +1221,16 @@ namespace OpenSim.Region.Physics.OdePlugin return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid); } + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) + { +#if SPAM + m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); +#endif + + return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); + } + public override float TimeDilation { get { return m_timeDilation; } -- cgit v1.1 From f6cbafcaf0f33b37d076d654e35c20990eb205d8 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 21 Mar 2012 02:39:16 +0000 Subject: Changes of PrimShapeType should now work with UbitOde ( almost untested ) --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 7434 ++++++++++++----------- 1 file changed, 3724 insertions(+), 3710 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index fd2f88f..71aec4c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1,3711 +1,3725 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -/* Revision 2011/12 by Ubit Umarov - * - * - */ - -/* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - */ - -//#define SPAM - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Threading; -using log4net; -using OpenMetaverse; -using OdeAPI; -using OpenSim.Framework; -using OpenSim.Region.Physics.Manager; - - -namespace OpenSim.Region.Physics.OdePlugin -{ - public class OdePrim : PhysicsActor - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - private bool m_isphysical; - private bool m_fakeisphysical; - private bool m_isphantom; - private bool m_fakeisphantom; - - protected bool m_building; - private Quaternion m_lastorientation = new Quaternion(); - private Quaternion _orientation; - - private Vector3 _position; - private Vector3 _velocity; - private Vector3 _torque; - private Vector3 m_lastVelocity; - private Vector3 m_lastposition; - private Vector3 m_rotationalVelocity; - private Vector3 _size; - private Vector3 _acceleration; - private Vector3 m_angularlock = Vector3.One; - private IntPtr Amotor = IntPtr.Zero; - - private Vector3 m_force; - private Vector3 m_forceacc; - private Vector3 m_angularForceacc; - - private Vector3 m_PIDTarget; - private float m_PIDTau; - private float PID_D = 35f; - private float PID_G = 25f; - private bool m_usePID; - - // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), - // and are for non-VEHICLES only. - - private float m_PIDHoverHeight; - private float m_PIDHoverTau; - private bool m_useHoverPID; - private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; - private float m_targetHoverHeight; - private float m_groundHeight; - private float m_waterHeight; - private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - - private int body_autodisable_frames = 20; - - private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom - | CollisionCategories.Space - | CollisionCategories.Body - | CollisionCategories.Character - ); -// private bool m_collidesLand = true; - private bool m_collidesWater; - public bool m_returnCollisions; - private bool m_softcolide; - - private bool m_NoColide; // for now only for internal use for bad meshs - - // Default we're a Geometry - private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); - - // Default, Collide with Other Geometries, spaces and Bodies - private CollisionCategories m_collisionFlags = m_default_collisionFlags; - - public bool m_disabled; - - - public uint m_localID; - - private PrimitiveBaseShape _pbs; - public OdeScene _parent_scene; - - /// - /// The physics space which contains prim geometry - /// - public IntPtr m_targetSpace = IntPtr.Zero; - - public IntPtr prim_geom; - public IntPtr _triMeshData; - - private PhysicsActor _parent; - - private List childrenPrim = new List(); - - private bool m_iscolliding; - - public bool m_isSelected; - private bool m_delaySelect; - private bool m_lastdoneSelected; - public bool m_outbounds; - - internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - - private bool m_throttleUpdates; - private int throttleCounter; - public float m_collisionscore; - int m_colliderfilter = 0; - - public IntPtr collide_geom; // for objects: geom if single prim space it linkset - - private float m_density = 10.000006836f; // Aluminum g/cm3; - private byte m_shapetype; - public bool _zeroFlag; - private bool m_lastUpdateSent; - - public IntPtr Body = IntPtr.Zero; - public String Name { get; private set; } - private Vector3 _target_velocity; - - public Vector3 primOOBsize; // prim real dimensions from mesh - public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb - public float primOOBradiusSQ; - public d.Mass primdMass; // prim inertia information on it's own referencial - float primMass; // prim own mass - float _mass; // object mass acording to case - private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb - - public int givefakepos = 0; - private Vector3 fakepos; - public int givefakeori = 0; - private Quaternion fakeori; - - public int m_eventsubscription; - private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); - - public volatile bool childPrim; - - public ODEDynamics m_vehicle; - - internal int m_material = (int)Material.Wood; - private float mu; - private float bounce; - - /// - /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. - /// - public override bool IsPhysical // this is not reliable for internal use - { - get { return m_fakeisphysical; } - set - { - m_fakeisphysical = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - - if (!value) // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - AddChange(changes.Physical, value); - } - } - - public override bool Phantom // this is not reliable for internal use - { - get { return m_fakeisphantom; } - set - { - m_fakeisphantom = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - - AddChange(changes.Phantom, value); - } - } - - public override bool Building // this is not reliable for internal use - { - get { return m_building; } - set - { - if (value) - m_building = true; - AddChange(changes.building, value); - } - } - - public override void getContactData(ref ContactData cdata) - { - cdata.mu = mu; - cdata.bounce = bounce; - - // cdata.softcolide = m_softcolide; - cdata.softcolide = false; - - if (m_isphysical) - { - ODEDynamics veh; - if (_parent != null) - veh = ((OdePrim)_parent).m_vehicle; - else - veh = m_vehicle; - - if (veh != null && veh.Type != Vehicle.TYPE_NONE) - cdata.mu *= veh.FrictionFactor; - } - } - - public override int PhysicsActorType - { - get { return (int)ActorTypes.Prim; } - set { return; } - } - - public override bool SetAlwaysRun - { - get { return false; } - set { return; } - } - - public override uint LocalID - { - get - { - return m_localID; - } - set - { - //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); - m_localID = value; - } - } - - public override bool Grabbed - { - set { return; } - } - - public override bool Selected - { - set - { - if (value) - m_isSelected = value; // if true set imediatly to stop moves etc - AddChange(changes.Selected, value); - } - } - - public override bool Flying - { - // no flying prims for you - get { return false; } - set { } - } - - public override bool IsColliding - { - get { return m_iscolliding; } - set - { - if (value) - { - m_colliderfilter += 2; - if (m_colliderfilter > 2) - m_colliderfilter = 2; - } - else - { - m_colliderfilter--; - if (m_colliderfilter < 0) - m_colliderfilter = 0; - } - - if (m_colliderfilter == 0) - { - m_softcolide = false; - m_iscolliding = false; - } - else - m_iscolliding = true; - } - } - - public override bool CollidingGround - { - get { return false; } - set { return; } - } - - public override bool CollidingObj - { - get { return false; } - set { return; } - } - - public override bool ThrottleUpdates - { - get { return m_throttleUpdates; } - set { m_throttleUpdates = value; } - } - - public override bool Stopped - { - get { return _zeroFlag; } - } - - public override Vector3 Position - { - get - { - if (givefakepos > 0) - return fakepos; - else - return _position; - } - - set - { - fakepos = value; - givefakepos++; - AddChange(changes.Position, value); - } - } - - public override Vector3 Size - { - get { return _size; } - set - { - if (value.IsFinite()) - { - AddChange(changes.Size, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); - } - } - } - - public override float Mass - { - get { return _mass; } - } - - public override Vector3 Force - { - //get { return Vector3.Zero; } - get { return m_force; } - set - { - if (value.IsFinite()) - { - AddChange(changes.Force, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); - } - } - } - - public override void SetVolumeDetect(int param) - { - AddChange(changes.VolumeDtc, (param != 0)); - } - - public override Vector3 GeometricCenter - { - get - { - return Vector3.Zero; - } - } - - public override Vector3 CenterOfMass - { - get - { - d.Vector3 dtmp; - if (IsPhysical && !childPrim && Body != IntPtr.Zero) - { - dtmp = d.BodyGetPosition(Body); - return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - } - else if (prim_geom != IntPtr.Zero) - { - d.Quaternion dq; - d.GeomCopyQuaternion(prim_geom, out dq); - Quaternion q; - q.X = dq.X; - q.Y = dq.Y; - q.Z = dq.Z; - q.W = dq.W; - - Vector3 vtmp = primOOBoffset * q; - dtmp = d.GeomGetPosition(prim_geom); - return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); - } - else - return Vector3.Zero; - } - } - /* - public override Vector3 PrimOOBsize - { - get - { - return primOOBsize; - } - } - - public override Vector3 PrimOOBoffset - { - get - { - return primOOBoffset; - } - } - - public override float PrimOOBRadiusSQ - { - get - { - return primOOBradiusSQ; - } - } - */ - public override PrimitiveBaseShape Shape - { - set - { - AddChange(changes.Shape, value); - } - } - - public override Vector3 Velocity - { - get - { - if (_zeroFlag) - return Vector3.Zero; - return _velocity; - } - set - { - if (value.IsFinite()) - { - AddChange(changes.Velocity, value); - // _velocity = value; - - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); - } - - } - } - - public override Vector3 Torque - { - get - { - if (!IsPhysical || Body == IntPtr.Zero) - return Vector3.Zero; - - return _torque; - } - - set - { - if (value.IsFinite()) - { - AddChange(changes.Torque, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); - } - } - } - - public override float CollisionScore - { - get { return m_collisionscore; } - set { m_collisionscore = value; } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override Quaternion Orientation - { - get - { - if (givefakeori > 0) - return fakeori; - else - - return _orientation; - } - set - { - if (QuaternionIsFinite(value)) - { - fakeori = value; - givefakeori++; - AddChange(changes.Orientation, value); - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); - - } - } - - public override Vector3 Acceleration - { - get { return _acceleration; } - set { } - } - - public override Vector3 RotationalVelocity - { - get - { - Vector3 pv = Vector3.Zero; - if (_zeroFlag) - return pv; - m_lastUpdateSent = false; - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - return pv; - - return m_rotationalVelocity; - } - set - { - if (value.IsFinite()) - { - m_rotationalVelocity = value; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); - } - } - } - - - public override float Buoyancy - { - get { return m_buoyancy; } - set - { - m_buoyancy = value; - } - } - - public override bool FloatOnWater - { - set - { - AddChange(changes.CollidesWater, value); - } - } - - public override Vector3 PIDTarget - { - set - { - if (value.IsFinite()) - { - m_PIDTarget = value; - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); - } - } - - public override bool PIDActive { set { m_usePID = value; } } - public override float PIDTau { set { m_PIDTau = value; } } - - public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } - public override bool PIDHoverActive { set { m_useHoverPID = value; } } - public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } - public override float PIDHoverTau { set { m_PIDHoverTau = value; } } - - public override Quaternion APIDTarget { set { return; } } - - public override bool APIDActive { set { return; } } - - public override float APIDStrength { set { return; } } - - public override float APIDDamping { set { return; } } - - public override int VehicleType - { - // we may need to put a fake on this - get - { - if (m_vehicle == null) - return (int)Vehicle.TYPE_NONE; - else - return (int)m_vehicle.Type; - } - set - { - AddChange(changes.VehicleType, value); - } - } - - public override void VehicleFloatParam(int param, float value) - { - strVehicleFloatParam fp = new strVehicleFloatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleFloatParam, fp); - } - - public override void VehicleVectorParam(int param, Vector3 value) - { - strVehicleVectorParam fp = new strVehicleVectorParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleVectorParam, fp); - } - - public override void VehicleRotationParam(int param, Quaternion value) - { - strVehicleQuatParam fp = new strVehicleQuatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleRotationParam, fp); - } - - public override void VehicleFlags(int param, bool value) - { - strVehicleBoolParam bp = new strVehicleBoolParam(); - bp.param = param; - bp.value = value; - AddChange(changes.VehicleFlags, bp); - } - - public override void SetVehicle(object vdata) - { - AddChange(changes.SetVehicle, vdata); - } - public void SetAcceleration(Vector3 accel) - { - _acceleration = accel; - } - - public override void AddForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); - } - //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); - } - - public override void AddAngularForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); - } - } - - public override void CrossingFailure() - { - if (m_outbounds) - { - _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); - _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); - _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); - - m_lastposition = _position; - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - m_lastVelocity = _velocity; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - - if(Body != IntPtr.Zero) - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - if (prim_geom != IntPtr.Zero) - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - - m_outbounds = false; - changeDisable(false); - base.RequestPhysicsterseUpdate(); - } - } - - public override void SetMomentum(Vector3 momentum) - { - } - - public override void SetMaterial(int pMaterial) - { - m_material = pMaterial; - mu = _parent_scene.m_materialContactsData[pMaterial].mu; - bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; - } - - public void setPrimForRemoval() - { - AddChange(changes.Remove, null); - } - - public override void link(PhysicsActor obj) - { - AddChange(changes.Link, obj); - } - - public override void delink() - { - AddChange(changes.DeLink, null); - } - - public override void LockAngularMotion(Vector3 axis) - { - // reverse the zero/non zero values for ODE. - if (axis.IsFinite()) - { - axis.X = (axis.X > 0) ? 1f : 0f; - axis.Y = (axis.Y > 0) ? 1f : 0f; - axis.Z = (axis.Z > 0) ? 1f : 0f; - m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - AddChange(changes.AngLock, axis); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); - } - } - - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - _parent_scene.AddCollisionEventReporting(this); - } - - public override void UnSubscribeEvents() - { - _parent_scene.RemoveCollisionEventReporting(this); - m_eventsubscription = 0; - } - - public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) - { - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); - - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - } - - public void SendCollisions() - { - if (CollisionEventsThisFrame == null) - return; - - base.SendCollisionUpdate(CollisionEventsThisFrame); - - if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) - CollisionEventsThisFrame = null; - else - CollisionEventsThisFrame = new CollisionEventUpdate(); - } - - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } - - - public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) - { - Name = primName; - LocalID = plocalID; - - m_vehicle = null; - - if (!pos.IsFinite()) - { - pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), - parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); - } - _position = pos; - givefakepos = 0; - - PID_D = parent_scene.bodyPIDD; - PID_G = parent_scene.bodyPIDG; - m_density = parent_scene.geomDefaultDensity; - // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; - body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - - prim_geom = IntPtr.Zero; - collide_geom = IntPtr.Zero; - Body = IntPtr.Zero; - - if (!size.IsFinite()) - { - size = new Vector3(0.5f, 0.5f, 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); - } - - if (size.X <= 0) size.X = 0.01f; - if (size.Y <= 0) size.Y = 0.01f; - if (size.Z <= 0) size.Z = 0.01f; - - _size = size; - - if (!QuaternionIsFinite(rotation)) - { - rotation = Quaternion.Identity; - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); - } - - _orientation = rotation; - givefakeori = 0; - - _pbs = pbs; - - _parent_scene = parent_scene; - m_targetSpace = IntPtr.Zero; - - if (pos.Z < 0) - { - m_isphysical = false; - } - else - { - m_isphysical = pisPhysical; - } - m_fakeisphysical = m_isphysical; - - m_isVolumeDetect = false; - - m_force = Vector3.Zero; - - m_iscolliding = false; - m_colliderfilter = 0; - m_softcolide = true; - m_NoColide = false; - - hasOOBoffsetFromMesh = false; - _triMeshData = IntPtr.Zero; - - m_shapetype = _shapeType; - - m_lastdoneSelected = false; - m_isSelected = false; - m_delaySelect = false; - - m_isphantom = pisPhantom; - m_fakeisphantom = pisPhantom; - - mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; - bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; - - CalcPrimBodyData(); - - m_building = true; // control must set this to false when done - - AddChange(changes.Add, null); - } - - private void resetCollisionAccounting() - { - m_collisionscore = 0; - } - - private void createAMotor(Vector3 axis) - { - if (Body == IntPtr.Zero) - return; - - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - - int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); - - if (axisnum <= 0) - return; - - // stop it - d.BodySetTorque(Body, 0, 0, 0); - d.BodySetAngularVel(Body, 0, 0, 0); - - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - - d.JointSetAMotorMode(Amotor, 0); - - d.JointSetAMotorNumAxes(Amotor, axisnum); - - // get current orientation to lock - - d.Quaternion dcur = d.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; - Vector3 ax; - - int i = 0; - int j = 0; - if (axis.X == 0) - { - ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X - // ODE should do this with axis relative to body 1 but seems to fail - d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, 0, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); - i++; - j = 256; // move to next axis set - } - - if (axis.Y == 0) - { - ax = (new Vector3(0, 1, 0)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - i++; - j += 256; - } - - if (axis.Z == 0) - { - ax = (new Vector3(0, 0, 1)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - } - } - - private bool setMesh(OdeScene parent_scene) - { - if (Body != IntPtr.Zero) - { - if (childPrim) - { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this, false); - } - } - else - { - DestroyBody(); - } - } - - bool convex; - if (m_shapetype == 0) - convex = false; - else - convex = true; - - IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex); - if (mesh == null) - { - m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); - return false; - } - - IntPtr vertices, indices; - int vertexCount, indexCount; - int vertexStride, triStride; - - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage - - if (vertexCount == 0 || indexCount == 0) - { - m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", - Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); - mesh.releaseSourceMeshData(); - return false; - } - - primOOBoffset = mesh.GetCentroid(); - hasOOBoffsetFromMesh = true; - - mesh.releaseSourceMeshData(); - - IntPtr geo = IntPtr.Zero; - - try - { - _triMeshData = d.GeomTriMeshDataCreate(); - - d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); - d.GeomTriMeshDataPreprocess(_triMeshData); - - _parent_scene.waitForSpaceUnlock(m_targetSpace); - geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); - } - - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - return false; - } - - SetGeom(geo); - return true; - } - - private void SetGeom(IntPtr geom) - { - prim_geom = geom; - //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - { - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCollideBits(prim_geom, 0); - d.GeomDisable(prim_geom); - } - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - - CalcPrimBodyData(); - - _parent_scene.geom_name_map[prim_geom] = Name; - _parent_scene.actor_name_map[prim_geom] = this; - - } - else - m_log.Warn("Setting bad Geom"); - } - - - /// - /// Create a geometry for the given mesh in the given target space. - /// - /// - /// If null, then a mesh is used that is based on the profile shape data. - private void CreateGeom() - { - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - - bool haveMesh = false; - hasOOBoffsetFromMesh = false; - m_NoColide = false; - - if (_parent_scene.needsMeshing(_pbs)) - { - haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims - if (!haveMesh) - m_NoColide = true; - } - - if (!haveMesh) - { - if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 - && _size.X == _size.Y && _size.Y == _size.Z) - { // it's a sphere - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); - return; - } - } - else - {// do it as a box - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - //Console.WriteLine(" CreateGeom 4"); - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); - } - catch (Exception e) - { - m_log.Warn("[PHYSICS]: Create box failed: {0}", e); - return; - } - } - } - } - - /// - /// Set a new geometry for this prim. - /// - /// - private void RemoveGeom() - { - if (prim_geom != IntPtr.Zero) - { - _parent_scene.geom_name_map.Remove(prim_geom); - _parent_scene.actor_name_map.Remove(prim_geom); - try - { - d.GeomDestroy(prim_geom); - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - } - // catch (System.AccessViolationException) - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); - } - - prim_geom = IntPtr.Zero; - } - else - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); - } - Body = IntPtr.Zero; - hasOOBoffsetFromMesh = false; - CalcPrimBodyData(); - } - - private void ChildSetGeom(OdePrim odePrim) - { - // well.. - DestroyBody(); - MakeBody(); - } - - //sets non physical prim m_targetSpace to right space in spaces grid for static prims - // should only be called for non physical prims unless they are becoming non physical - private void SetInStaticSpace(OdePrim prim) - { - IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); - prim.m_targetSpace = targetSpace; - d.GeomEnable(prim_geom); - } - - public void enableBodySoft() - { - if (!childPrim && !m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - m_collisionFlags = CollisionCategories.Land; - } - else - { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prm.prim_geom); - } - } - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prim_geom); - } - d.BodyEnable(Body); - } - } - m_disabled = false; - resetCollisionAccounting(); // this sets m_disable to false - } - - private void disableBodySoft() - { - m_disabled = true; - if (!childPrim) - { - if (m_isphysical && Body != IntPtr.Zero) - { - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prm.prim_geom); - } - } - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prim_geom); - } - - d.BodyDisable(Body); - } - } - } - - private void MakeBody() - { - if (!m_isphysical) // only physical get bodies - return; - - if (childPrim) // child prims don't get bodies; - return; - - if (m_building) - return; - - if (prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); - return; - } - - if (Body != IntPtr.Zero) - { - d.BodyDestroy(Body); - Body = IntPtr.Zero; - m_log.Warn("[PHYSICS]: MakeBody called having a body"); - } - - - if (d.GeomGetBody(prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); - } - - d.Matrix3 mymat = new d.Matrix3(); - d.Quaternion myrot = new d.Quaternion(); - d.Mass objdmass = new d.Mass { }; - - Body = d.BodyCreate(_parent_scene.world); - - DMassDup(ref primdMass, out objdmass); - - // rotate inertia - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); - - // set the body rotation and position - d.BodySetRotation(Body, ref mymat); - - // recompute full object inertia if needed - if (childrenPrim.Count > 0) - { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - d.Mass tmpdmass = new d.Mass { }; - Vector3 rcm; - - rcm.X = _position.X + objdmass.c.X; - rcm.Y = _position.Y + objdmass.c.Y; - rcm.Z = _position.Z + objdmass.c.Z; - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); - continue; - } - - DMassCopy(ref prm.primdMass, ref tmpdmass); - - // apply prim current rotation to inertia - quat.X = prm._orientation.X; - quat.Y = prm._orientation.Y; - quat.Z = prm._orientation.Z; - quat.W = prm._orientation.W; - d.RfromQ(out mat, ref quat); - d.MassRotate(ref tmpdmass, ref mat); - - Vector3 ppos = prm._position; - ppos.X += tmpdmass.c.X - rcm.X; - ppos.Y += tmpdmass.c.Y - rcm.Y; - ppos.Z += tmpdmass.c.Z - rcm.Z; - - // refer inertia to root prim center of mass position - d.MassTranslate(ref tmpdmass, - ppos.X, - ppos.Y, - ppos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia - // fix prim colision cats - - if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); - } - - d.GeomClearOffset(prm.prim_geom); - d.GeomSetBody(prm.prim_geom, Body); - prm.Body = Body; - d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation - } - } - } - - d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset - // associate root geom with body - d.GeomSetBody(prim_geom, Body); - - d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); - d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); - - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - myrot.W = -myrot.W; - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; - - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode(Body, false); - - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - // d.BodySetLinearDampingThreshold(Body, 0.01f); - // d.BodySetAngularDampingThreshold(Body, 0.001f); - d.BodySetDamping(Body, .002f, .002f); - - - if (m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - if (d.SpaceQuery(m_targetSpace, prim_geom)) - d.SpaceRemove(m_targetSpace, prim_geom); - } - - - if (childrenPrim.Count == 0) - { - collide_geom = prim_geom; - m_targetSpace = _parent_scene.ActiveSpace; - d.SpaceAdd(m_targetSpace, prim_geom); - } - else - { - m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); - d.HashSpaceSetLevels(m_targetSpace, -2, 8); - d.SpaceSetSublevel(m_targetSpace, 3); - d.SpaceSetCleanup(m_targetSpace, false); - d.SpaceAdd(m_targetSpace, prim_geom); - collide_geom = m_targetSpace; - } - - if (m_delaySelect) - { - m_isSelected = true; - m_delaySelect = false; - } - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom == IntPtr.Zero) - continue; - - Vector3 ppos = prm._position; - d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position - - if (prm.m_targetSpace != m_targetSpace) - { - if (prm.m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); - if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) - d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); - } - prm.m_targetSpace = m_targetSpace; - d.SpaceAdd(m_targetSpace, prm.prim_geom); - } - - if (m_isSelected || m_disabled) - { - prm.m_collisionCategories &= ~CollisionCategories.Body; - prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); - d.GeomDisable(prm.prim_geom); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - prm.m_collisionCategories = 0; - prm.m_collisionFlags = CollisionCategories.Land; - } - else - { - prm.m_collisionCategories |= CollisionCategories.Body; - prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - d.GeomEnable(prm.prim_geom); - } - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - d.GeomEnable(prm.prim_geom); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); - } - prm.m_collisionscore = 0; - - if(!m_disabled) - prm.m_disabled = false; - - _parent_scene.addActivePrim(prm); - } - } - - // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) - { - createAMotor(m_angularlock); - } - - if (m_isSelected || m_disabled) - { - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); - - d.GeomDisable(prim_geom); - d.BodyDisable(Body); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - m_collisionFlags = CollisionCategories.Land; - } - else - { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - - d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - } - - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - - m_collisionscore = 0; - - m_softcolide = true; - _parent_scene.addActivePrim(this); - _parent_scene.addActiveGroups(this); - } - - private void DestroyBody() - { - if (Body != IntPtr.Zero) - { - _parent_scene.remActivePrim(this); - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - UpdateDataFromGeom(); - d.GeomSetBody(prim_geom, IntPtr.Zero); - SetInStaticSpace(this); - } - - if (!childPrim) - { - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - _parent_scene.remActivePrim(prm); - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - prm.UpdateDataFromGeom(); - SetInStaticSpace(prm); - } - prm.Body = IntPtr.Zero; - prm._mass = prm.primMass; - prm.m_collisionscore = 0; - } - } - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - _parent_scene.remActiveGroup(this); - d.BodyDestroy(Body); - } - Body = IntPtr.Zero; - } - _mass = primMass; - m_collisionscore = 0; - } - - #region Mass Calculation - - private float CalculatePrimVolume() - { - float volume = _size.X * _size.Y * _size.Z; // default - float tmp; - - float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - - switch (_pbs.ProfileShape) - { - case ProfileShape.Square: - // default box - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - - hollowVolume *= (0.5f * .5f); - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - //a tube - - volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume * tmp * tmp; - - if (hollowAmount > 0.0) - { - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - hollowVolume *= 0.5f * 0.5f; - break; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - break; - - case ProfileShape.Circle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.78539816339f; // elipse base - - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - case ProfileShape.HalfCircle: - if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.52359877559829887307710723054658f; - } - break; - - case ProfileShape.EquilateralTriangle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.32475953f; - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.32475953f; - volume *= 0.01f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - default: - break; - } - - float taperX1; - float taperY1; - float taperX; - float taperY; - float pathBegin; - float pathEnd; - float profileBegin; - float profileEnd; - - if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) - { - taperX1 = _pbs.PathScaleX * 0.01f; - if (taperX1 > 1.0f) - taperX1 = 2.0f - taperX1; - taperX = 1.0f - taperX1; - - taperY1 = _pbs.PathScaleY * 0.01f; - if (taperY1 > 1.0f) - taperY1 = 2.0f - taperY1; - taperY = 1.0f - taperY1; - } - else - { - taperX = _pbs.PathTaperX * 0.01f; - if (taperX < 0.0f) - taperX = -taperX; - taperX1 = 1.0f - taperX; - - taperY = _pbs.PathTaperY * 0.01f; - if (taperY < 0.0f) - taperY = -taperY; - taperY1 = 1.0f - taperY; - } - - volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); - - pathBegin = (float)_pbs.PathBegin * 2.0e-5f; - pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; - volume *= (pathEnd - pathBegin); - - // this is crude aproximation - profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; - profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; - volume *= (profileEnd - profileBegin); - - return volume; - } - - - private void CalcPrimBodyData() - { - float volume; - - if (prim_geom == IntPtr.Zero) - { - // Ubit let's have a initial basic OOB - primOOBsize.X = _size.X; - primOOBsize.Y = _size.Y; - primOOBsize.Z = _size.Z; - primOOBoffset = Vector3.Zero; - } - else - { - d.AABB AABB; - d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom - - primOOBsize.X = (AABB.MaxX - AABB.MinX); - primOOBsize.Y = (AABB.MaxY - AABB.MinY); - primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); - if (!hasOOBoffsetFromMesh) - { - primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; - primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; - primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; - } - } - - // also its own inertia and mass - // keep using basic shape mass for now - volume = CalculatePrimVolume(); - - primMass = m_density * volume; - - if (primMass <= 0) - primMass = 0.0001f;//ckrinke: Mass must be greater then zero. - if (primMass > _parent_scene.maximumMassObject) - primMass = _parent_scene.maximumMassObject; - - _mass = primMass; // just in case - - d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); - - d.MassTranslate(ref primdMass, - primOOBoffset.X, - primOOBoffset.Y, - primOOBoffset.Z); - - primOOBsize *= 0.5f; // let obb size be a corner coords - primOOBradiusSQ = primOOBsize.LengthSquared(); - } - - - #endregion - - - /// - /// Add a child prim to this parent prim. - /// - /// Child prim - // I'm the parent - // prim is the child - public void ParentPrim(OdePrim prim) - { - //Console.WriteLine("ParentPrim " + m_primName); - if (this.m_localID != prim.m_localID) - { - DestroyBody(); // for now we need to rebuil entire object on link change - - lock (childrenPrim) - { - // adopt the prim - if (!childrenPrim.Contains(prim)) - childrenPrim.Add(prim); - - // see if this prim has kids and adopt them also - // should not happen for now - foreach (OdePrim prm in prim.childrenPrim) - { - if (!childrenPrim.Contains(prm)) - { - if (prm.Body != IntPtr.Zero) - { - if (prm.prim_geom != IntPtr.Zero) - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - if (prm.Body != prim.Body) - prm.DestroyBody(); // don't loose bodies around - prm.Body = IntPtr.Zero; - } - - childrenPrim.Add(prm); - prm._parent = this; - } - } - } - //Remove old children from the prim - prim.childrenPrim.Clear(); - - if (prim.Body != IntPtr.Zero) - { - if (prim.prim_geom != IntPtr.Zero) - d.GeomSetBody(prim.prim_geom, IntPtr.Zero); - prim.DestroyBody(); // don't loose bodies around - prim.Body = IntPtr.Zero; - } - - prim.childPrim = true; - prim._parent = this; - - MakeBody(); // full nasty reconstruction - } - } - - private void UpdateChildsfromgeom() - { - if (childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.UpdateDataFromGeom(); - } - } - - private void UpdateDataFromGeom() - { - if (prim_geom != IntPtr.Zero) - { - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - d.Quaternion qtmp = new d.Quaternion { }; - d.GeomCopyQuaternion(prim_geom, out qtmp); - _orientation.W = qtmp.W; - _orientation.X = qtmp.X; - _orientation.Y = qtmp.Y; - _orientation.Z = qtmp.Z; - } - } - - private void ChildDelink(OdePrim odePrim, bool remakebodies) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) // delinking the root prim - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) - { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); - } - if (newroot != null) - { - newroot.childPrim = false; - newroot._parent = null; - if (remakebodies) - newroot.MakeBody(); - } - } - } - - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - // odePrim.UpdateDataFromGeom(); - if (remakebodies) - odePrim.MakeBody(); - } - } - if (remakebodies) - MakeBody(); - } - - protected void ChildRemove(OdePrim odePrim, bool reMakeBody) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) - { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); - } - if (newroot != null) - { - newroot.childPrim = false; - newroot._parent = null; - newroot.MakeBody(); - } - } - if (reMakeBody) - MakeBody(); - return; - } - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - if (reMakeBody) - odePrim.MakeBody(); - } - } - MakeBody(); - } - - #region changes - - private void changeadd() - { - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - - if (!m_isphysical) - SetInStaticSpace(this); - } - - if (m_isphysical && Body == IntPtr.Zero) - { - MakeBody(); - } - } - - private void changeAngularLock(Vector3 newLock) - { - // do we have a Physical object? - if (Body != IntPtr.Zero) - { - //Check that we have a Parent - //If we have a parent then we're not authorative here - if (_parent == null) - { - if (!newLock.ApproxEquals(Vector3.One, 0f)) - { - createAMotor(newLock); - } - else - { - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - } - } - } - // Store this for later in case we get turned into a separate body - m_angularlock = newLock; - } - - private void changeLink(OdePrim NewParent) - { - if (_parent == null && NewParent != null) - { - NewParent.ParentPrim(this); - } - else if (_parent != null) - { - if (_parent is OdePrim) - { - if (NewParent != _parent) - { - (_parent as OdePrim).ChildDelink(this, false); // for now... - childPrim = false; - - if (NewParent != null) - { - NewParent.ParentPrim(this); - } - } - } - } - _parent = NewParent; - } - - - private void Stop() - { - if (!childPrim) - { - m_force = Vector3.Zero; - m_forceacc = Vector3.Zero; - m_angularForceacc = Vector3.Zero; - _torque = Vector3.Zero; - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - _target_velocity = Vector3.Zero; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - } - - if (Body != IntPtr.Zero) - { - d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetTorque(Body, 0f, 0f, 0f); - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetAngularVel(Body, 0f, 0f, 0f); - } - } - - - private void changePhantomStatus(bool newval) - { - m_isphantom = newval; - - if (m_isSelected) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - if (m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; // should never happen - } - - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; - } - } - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - if(!m_isSelected) - d.GeomEnable(prm.prim_geom); - } - } - } - - if (!m_disabled && prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - if(!m_isSelected) - d.GeomEnable(prim_geom); - } - } - - private void changeSelectedStatus(bool newval) - { - if (m_lastdoneSelected == newval) - return; - - m_lastdoneSelected = newval; - DoSelectedStatus(newval); - } - - private void CheckDelaySelect() - { - if (m_delaySelect) - { - DoSelectedStatus(m_isSelected); - } - } - - private void DoSelectedStatus(bool newval) - { - m_isSelected = newval; - Stop(); - - if (newval) - { - if (!childPrim && Body != IntPtr.Zero) - d.BodyDisable(Body); - - if (m_delaySelect || m_isphysical) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != null) - { - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prm.prim_geom); - } - prm.m_delaySelect = false; - } - } - - if (prim_geom != null) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prim_geom); - } - - m_delaySelect = false; - } - else if(!m_isphysical) - { - m_delaySelect = true; - } - } - else - { - if (!childPrim && Body != IntPtr.Zero && !m_disabled) - d.BodyEnable(Body); - - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - if(m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; - } - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; - } - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prm.prim_geom); - } - prm.m_delaySelect = false; - prm.m_softcolide = true; - } - } - - if (!m_disabled && prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prim_geom); - } - - m_delaySelect = false; - m_softcolide = true; - } - - resetCollisionAccounting(); - } - - private void changePosition(Vector3 newPos) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _position = newPos; - } - } - else - { - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (prim_geom != IntPtr.Zero) - { - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } - } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; - // changeSelectedStatus(); - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changeOrientation(Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _orientation = newOri; - } - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } - } - } - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim && m_building) // inertia is messed, must rebuild - { - _position = newPos; - _orientation = newOri; - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); - // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } - - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } - } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - - m_softcolide = true; - resetCollisionAccounting(); - } - - - private void changeDisable(bool disable) - { - if (disable) - { - if (!m_disabled) - disableBodySoft(); - } - else - { - if (m_disabled) - enableBodySoft(); - } - } - - private void changePhysicsStatus(bool NewStatus) - { - CheckDelaySelect(); - - m_isphysical = NewStatus; - - if (!childPrim) - { - if (NewStatus) - { - if (Body == IntPtr.Zero) - MakeBody(); - } - else - { - if (Body != IntPtr.Zero) - { - DestroyBody(); - } - Stop(); - } - } - - resetCollisionAccounting(); - } - - private void changeprimsizeshape() - { - CheckDelaySelect(); - - OdePrim parent = (OdePrim)_parent; - - bool chp = childPrim; - - if (chp) - { - if (parent != null) - { - parent.DestroyBody(); - } - } - else - { - DestroyBody(); - } - - RemoveGeom(); - - // we don't need to do space calculation because the client sends a position update also. - if (_size.X <= 0) - _size.X = 0.01f; - if (_size.Y <= 0) - _size.Y = 0.01f; - if (_size.Z <= 0) - _size.Z = 0.01f; - // Construction of new prim - - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - } - - if (chp) - { - if (parent != null) - { - parent.MakeBody(); - } - } - else - MakeBody(); - - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changeSize(Vector3 newSize) - { - _size = newSize; - changeprimsizeshape(); - } - - private void changeShape(PrimitiveBaseShape newShape) - { - _pbs = newShape; - changeprimsizeshape(); - } - - private void changeFloatOnWater(bool newval) - { - m_collidesWater = newval; - - if (prim_geom != IntPtr.Zero && !m_isphantom) - { - if (m_collidesWater) - { - m_collisionFlags |= CollisionCategories.Water; - } - else - { - m_collisionFlags &= ~CollisionCategories.Water; - } - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - } - - private void changeSetTorque(Vector3 newtorque) - { - if (!m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - } - _torque = newtorque; - } - } - - private void changeForce(Vector3 force) - { - m_force = force; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - - private void changeAddForce(Vector3 force) - { - m_forceacc += force; - if (!m_isSelected) - { - lock (this) - { - //m_log.Info("[PHYSICS]: dequeing forcelist"); - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - - m_collisionscore = 0; - } - } - - private void changeAddAngularForce(Vector3 aforce) - { - m_angularForceacc += aforce; - if (!m_isSelected) - { - lock (this) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - m_collisionscore = 0; - } - } - - private void changevelocity(Vector3 newVel) - { - if (!m_isSelected) - { - if (Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); - } - //resetCollisionAccounting(); - } - _velocity = newVel; - } - - private void changeVolumedetetion(bool newVolDtc) - { - m_isVolumeDetect = newVolDtc; - } - - protected void changeBuilding(bool newbuilding) - { - if ((bool)newbuilding) - { - m_building = true; - if (!childPrim) - DestroyBody(); - } - else - { - m_building = false; - CheckDelaySelect(); - if (!childPrim) - MakeBody(); - } - if (!childPrim && childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.changeBuilding(m_building); // call directly - } - } - - public void changeSetVehicle(VehicleData vdata) - { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - m_vehicle.DoSetVehicle(vdata); - } - private void changeVehicleType(int value) - { - if (value == (int)Vehicle.TYPE_NONE) - { - if (m_vehicle != null) - m_vehicle = null; - } - else - { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - - m_vehicle.ProcessTypeChange((Vehicle)value); - } - } - - private void changeVehicleFloatParam(strVehicleFloatParam fp) - { - if (m_vehicle == null) - return; - - m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); - } - - private void changeVehicleVectorParam(strVehicleVectorParam vp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); - } - - private void changeVehicleRotationParam(strVehicleQuatParam qp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); - } - - private void changeVehicleFlags(strVehicleBoolParam bp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVehicleFlags(bp.param, bp.value); - } - - #endregion - - public void Move() - { - if (!childPrim && m_isphysical && Body != IntPtr.Zero && - !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) - // !m_disabled && !m_isSelected && !m_building && !m_outbounds) - { -// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 - - float timestep = _parent_scene.ODE_STEPSIZE; - - // check outside region - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - - if (lpos.Z < -100 || lpos.Z > 100000f) - { - m_outbounds = true; - - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - return; - } - - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if(lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if(lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } - - if(m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; - - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - base.RequestPhysicsterseUpdate(); - return; - } - - - float fx = 0; - float fy = 0; - float fz = 0; - - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(); - } - else - { - float m_mass = _mass; - - // fz = 0f; - //m_log.Info(m_collisionFlags.ToString()); - if (m_usePID) - { - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } // end if (m_usePID) - - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID) - { - //Console.WriteLine("Hover " + Name); - - // If we're using the PID controller, then we have no gravity - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; - - } // end switch (m_PIDHoverType) - - - _target_velocity = - new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - // ? d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } - else - { - float b = (1.0f - m_buoyancy); - fx = _parent_scene.gravityx * b; - fy = _parent_scene.gravityy * b; - fz = _parent_scene.gravityz * b; - } - - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; - - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; - - m_forceacc = Vector3.Zero; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - d.BodyAddForce(Body, fx, fy, fz); - //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - - Vector3 trq; - - trq = _torque; - trq += m_angularForceacc; - m_angularForceacc = Vector3.Zero; - if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) - { - d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); - } - - } - } - else - { // is not physical, or is not a body or is selected - // _zeroPosition = d.BodyGetPosition(Body); - return; - //Console.WriteLine("Nothing " + Name); - - } - } - - - public void UpdatePositionAndVelocity(float simulatedtime) - { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null && !m_disabled && !m_building && !m_outbounds) - { - if (Body != IntPtr.Zero) - { - Vector3 pv = Vector3.Zero; - bool lastZeroFlag = _zeroFlag; - - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - - - d.Quaternion ori; - d.GeomCopyQuaternion(prim_geom, out ori); - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 rotvel = d.BodyGetAngularVel(Body); - - if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) - && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) - && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) - && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) - && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) - && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) - ) - { - _zeroFlag = true; - //Console.WriteLine("ZFT 2"); - m_throttleUpdates = false; - } - else - { - //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); - _zeroFlag = false; - m_lastUpdateSent = false; - //m_throttleUpdates = false; - } - - if (_zeroFlag) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - m_rotationalVelocity = pv; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; - } - } - else - { - if (lastZeroFlag != _zeroFlag) - { - base.RequestPhysicsterseUpdate(); - } - - m_lastVelocity = _velocity; - - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - - _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - { - m_rotationalVelocity = pv; - } - else - { - m_rotationalVelocity.X = rotvel.X; - m_rotationalVelocity.Y = rotvel.Y; - m_rotationalVelocity.Z = rotvel.Z; - } - - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) - { - m_lastposition = _position; - m_lastorientation = _orientation; - base.RequestPhysicsterseUpdate(); - } - else - { - throttleCounter++; - } - } - } - else if (!m_lastUpdateSent || !_zeroFlag) - { - // Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; - - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; - } - } - } - } - - internal static bool QuaternionIsFinite(Quaternion q) - { - if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) - return false; - if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) - return false; - if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) - return false; - if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) - return false; - return true; - } - - internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) - { - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } - - private static void DMassDup(ref d.Mass src, out d.Mass dst) - { - dst = new d.Mass { }; - - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } - private void donullchange() - { - } - - public bool DoAChange(changes what, object arg) - { - if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) - { - return false; - } - - // nasty switch - switch (what) - { - case changes.Add: - changeadd(); - break; - case changes.Remove: - //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... - //When we return true, it destroys all of the prims in the linkset anyway - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildRemove(this, false); - } - else - ChildRemove(this, false); - - m_vehicle = null; - RemoveGeom(); - m_targetSpace = IntPtr.Zero; - if (m_eventsubscription > 0) - UnSubscribeEvents(); - return true; - - case changes.Link: - OdePrim tmp = (OdePrim)arg; - changeLink(tmp); - break; - - case changes.DeLink: - changeLink(null); - break; - - case changes.Position: - changePosition((Vector3)arg); - break; - - case changes.Orientation: - changeOrientation((Quaternion)arg); - break; - - case changes.PosOffset: - donullchange(); - break; - - case changes.OriOffset: - donullchange(); - break; - - case changes.Velocity: - changevelocity((Vector3)arg); - break; - - // case changes.Acceleration: - // changeacceleration((Vector3)arg); - // break; - // case changes.AngVelocity: - // changeangvelocity((Vector3)arg); - // break; - - case changes.Force: - changeForce((Vector3)arg); - break; - - case changes.Torque: - changeSetTorque((Vector3)arg); - break; - - case changes.AddForce: - changeAddForce((Vector3)arg); - break; - - case changes.AddAngForce: - changeAddAngularForce((Vector3)arg); - break; - - case changes.AngLock: - changeAngularLock((Vector3)arg); - break; - - case changes.Size: - changeSize((Vector3)arg); - break; - - case changes.Shape: - changeShape((PrimitiveBaseShape)arg); - break; - - case changes.CollidesWater: - changeFloatOnWater((bool)arg); - break; - - case changes.VolumeDtc: - changeVolumedetetion((bool)arg); - break; - - case changes.Phantom: - changePhantomStatus((bool)arg); - break; - - case changes.Physical: - changePhysicsStatus((bool)arg); - break; - - case changes.Selected: - changeSelectedStatus((bool)arg); - break; - - case changes.disabled: - changeDisable((bool)arg); - break; - - case changes.building: - changeBuilding((bool)arg); - break; - - case changes.VehicleType: - changeVehicleType((int)arg); - break; - - case changes.VehicleFlags: - changeVehicleFlags((strVehicleBoolParam) arg); - break; - - case changes.VehicleFloatParam: - changeVehicleFloatParam((strVehicleFloatParam) arg); - break; - - case changes.VehicleVectorParam: - changeVehicleVectorParam((strVehicleVectorParam) arg); - break; - - case changes.VehicleRotationParam: - changeVehicleRotationParam((strVehicleQuatParam) arg); - break; - - case changes.SetVehicle: - changeSetVehicle((VehicleData) arg); - break; - case changes.Null: - donullchange(); - break; - - default: - donullchange(); - break; - } - return false; - } - - public void AddChange(changes what, object arg) - { - _parent_scene.AddChange((PhysicsActor) this, what, arg); - } - - - private struct strVehicleBoolParam - { - public int param; - public bool value; - } - - private struct strVehicleFloatParam - { - public int param; - public float value; - } - - private struct strVehicleQuatParam - { - public int param; - public Quaternion value; - } - - private struct strVehicleVectorParam - { - public int param; - public Vector3 value; - } - } +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* Revision 2011/12 by Ubit Umarov + * + * + */ + +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_isphysical; + private bool m_fakeisphysical; + private bool m_isphantom; + private bool m_fakeisphantom; + + protected bool m_building; + private Quaternion m_lastorientation = new Quaternion(); + private Quaternion _orientation; + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _torque; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 _acceleration; + private Vector3 m_angularlock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; + + private Vector3 m_force; + private Vector3 m_forceacc; + private Vector3 m_angularForceacc; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private float PID_D = 35f; + private float PID_G = 25f; + private bool m_usePID; + + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private float m_targetHoverHeight; + private float m_groundHeight; + private float m_waterHeight; + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + + private int body_autodisable_frames = 20; + + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); +// private bool m_collidesLand = true; + private bool m_collidesWater; + public bool m_returnCollisions; + private bool m_softcolide; + + private bool m_NoColide; // for now only for internal use for bad meshs + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlags; + + public bool m_disabled; + + + public uint m_localID; + + private PrimitiveBaseShape _pbs; + public OdeScene _parent_scene; + + /// + /// The physics space which contains prim geometry + /// + public IntPtr m_targetSpace = IntPtr.Zero; + + public IntPtr prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent; + + private List childrenPrim = new List(); + + private bool m_iscolliding; + + public bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + public bool m_outbounds; + + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + private bool m_throttleUpdates; + private int throttleCounter; + public float m_collisionscore; + int m_colliderfilter = 0; + + public IntPtr collide_geom; // for objects: geom if single prim space it linkset + + private float m_density = 10.000006836f; // Aluminum g/cm3; + private byte m_shapetype; + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body = IntPtr.Zero; + public String Name { get; private set; } + private Vector3 _target_velocity; + + public Vector3 primOOBsize; // prim real dimensions from mesh + public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb + public float primOOBradiusSQ; + public d.Mass primdMass; // prim inertia information on it's own referencial + float primMass; // prim own mass + float _mass; // object mass acording to case + private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb + + public int givefakepos = 0; + private Vector3 fakepos; + public int givefakeori = 0; + private Quaternion fakeori; + + public int m_eventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + public volatile bool childPrim; + + public ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + private float mu; + private float bounce; + + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical // this is not reliable for internal use + { + get { return m_fakeisphysical; } + set + { + m_fakeisphysical = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + if (!value) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + AddChange(changes.Physical, value); + } + } + + public override bool Phantom // this is not reliable for internal use + { + get { return m_fakeisphantom; } + set + { + m_fakeisphantom = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + AddChange(changes.Phantom, value); + } + } + + public override bool Building // this is not reliable for internal use + { + get { return m_building; } + set + { + if (value) + m_building = true; + AddChange(changes.building, value); + } + } + + public override void getContactData(ref ContactData cdata) + { + cdata.mu = mu; + cdata.bounce = bounce; + + // cdata.softcolide = m_softcolide; + cdata.softcolide = false; + + if (m_isphysical) + { + ODEDynamics veh; + if (_parent != null) + veh = ((OdePrim)_parent).m_vehicle; + else + veh = m_vehicle; + + if (veh != null && veh.Type != Vehicle.TYPE_NONE) + cdata.mu *= veh.FrictionFactor; + } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Prim; } + set { return; } + } + + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + + public override uint LocalID + { + get + { + return m_localID; + } + set + { + //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); + m_localID = value; + } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set + { + if (value) + m_isSelected = value; // if true set imediatly to stop moves etc + AddChange(changes.Selected, value); + } + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + { + m_softcolide = false; + m_iscolliding = false; + } + else + m_iscolliding = true; + } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get + { + if (givefakepos > 0) + return fakepos; + else + return _position; + } + + set + { + fakepos = value; + givefakepos++; + AddChange(changes.Position, value); + } + } + + public override Vector3 Size + { + get { return _size; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Size, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + } + } + } + + public override float Mass + { + get { return _mass; } + } + + public override Vector3 Force + { + //get { return Vector3.Zero; } + get { return m_force; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Force, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + } + } + } + + public override void SetVolumeDetect(int param) + { + AddChange(changes.VolumeDtc, (param != 0)); + } + + public override Vector3 GeometricCenter + { + get + { + return Vector3.Zero; + } + } + + public override Vector3 CenterOfMass + { + get + { + d.Vector3 dtmp; + if (IsPhysical && !childPrim && Body != IntPtr.Zero) + { + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; + + Vector3 vtmp = primOOBoffset * q; + dtmp = d.GeomGetPosition(prim_geom); + return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); + } + else + return Vector3.Zero; + } + } + /* + public override Vector3 PrimOOBsize + { + get + { + return primOOBsize; + } + } + + public override Vector3 PrimOOBoffset + { + get + { + return primOOBoffset; + } + } + + public override float PrimOOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } + } + */ + public override PrimitiveBaseShape Shape + { + set + { + AddChange(changes.Shape, value); + } + } + + public override byte PhysicsShapeType + { + get + { + return m_shapetype; + } + set + { + m_shapetype = value; + AddChange(changes.Shape, null); + } + } + + public override Vector3 Velocity + { + get + { + if (_zeroFlag) + return Vector3.Zero; + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); + // _velocity = value; + + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); + } + + } + } + + public override Vector3 Torque + { + get + { + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return _torque; + } + + set + { + if (value.IsFinite()) + { + AddChange(changes.Torque, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); + } + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get + { + if (givefakeori > 0) + return fakeori; + else + + return _orientation; + } + set + { + if (QuaternionIsFinite(value)) + { + fakeori = value; + givefakeori++; + AddChange(changes.Orientation, value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); + + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + m_buoyancy = value; + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + public override int VehicleType + { + // we may need to put a fake on this + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + AddChange(changes.VehicleType, value); + } + } + + public override void VehicleFloatParam(int param, float value) + { + strVehicleFloatParam fp = new strVehicleFloatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleFloatParam, fp); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + strVehicleVectorParam fp = new strVehicleVectorParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); + } + + public override void VehicleRotationParam(int param, Quaternion value) + { + strVehicleQuatParam fp = new strVehicleQuatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleRotationParam, fp); + } + + public override void VehicleFlags(int param, bool value) + { + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); + } + + public override void SetVehicle(object vdata) + { + AddChange(changes.SetVehicle, vdata); + } + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + if (m_outbounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + m_lastposition = _position; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + if(Body != IntPtr.Zero) + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + if (prim_geom != IntPtr.Zero) + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + + m_outbounds = false; + changeDisable(false); + base.RequestPhysicsterseUpdate(); + } + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + mu = _parent_scene.m_materialContactsData[pMaterial].mu; + bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public void setPrimForRemoval() + { + AddChange(changes.Remove, null); + } + + public override void link(PhysicsActor obj) + { + AddChange(changes.Link, obj); + } + + public override void delink() + { + AddChange(changes.DeLink, null); + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axis); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + _parent_scene.RemoveCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) + CollisionEventsThisFrame = null; + else + CollisionEventsThisFrame = new CollisionEventUpdate(); + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) + { + Name = primName; + LocalID = plocalID; + + m_vehicle = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); + } + _position = pos; + givefakepos = 0; + + PID_D = parent_scene.bodyPIDD; + PID_G = parent_scene.bodyPIDG; + m_density = parent_scene.geomDefaultDensity; + // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; + Body = IntPtr.Zero; + + if (!size.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + } + + _orientation = rotation; + givefakeori = 0; + + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_colliderfilter = 0; + m_softcolide = true; + m_NoColide = false; + + hasOOBoffsetFromMesh = false; + _triMeshData = IntPtr.Zero; + + m_shapetype = _shapeType; + + m_lastdoneSelected = false; + m_isSelected = false; + m_delaySelect = false; + + m_isphantom = pisPhantom; + m_fakeisphantom = pisPhantom; + + mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + CalcPrimBodyData(); + + m_building = true; // control must set this to false when done + + AddChange(changes.Add, null); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + } + + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); + + if (axisnum <= 0) + return; + + // stop it + d.BodySetTorque(Body, 0, 0, 0); + d.BodySetAngularVel(Body, 0, 0, 0); + + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + + d.JointSetAMotorNumAxes(Amotor, axisnum); + + // get current orientation to lock + + d.Quaternion dcur = d.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; + Vector3 ax; + + int i = 0; + int j = 0; + if (axis.X == 0) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + // ODE should do this with axis relative to body 1 but seems to fail + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); + i++; + j = 256; // move to next axis set + } + + if (axis.Y == 0) + { + ax = (new Vector3(0, 1, 0)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + i++; + j += 256; + } + + if (axis.Z == 0) + { + ax = (new Vector3(0, 0, 1)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + } + } + + private bool setMesh(OdeScene parent_scene) + { + if (Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this, false); + } + } + else + { + DestroyBody(); + } + } + + bool convex; + if (m_shapetype == 0) + convex = false; + else + convex = true; + + IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex); + if (mesh == null) + { + m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); + return false; + } + + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", + Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); + mesh.releaseSourceMeshData(); + return false; + } + + primOOBoffset = mesh.GetCentroid(); + hasOOBoffsetFromMesh = true; + + mesh.releaseSourceMeshData(); + + IntPtr geo = IntPtr.Zero; + + try + { + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + return false; + } + + SetGeom(geo); + return true; + } + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + { + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + CalcPrimBodyData(); + + _parent_scene.geom_name_map[prim_geom] = Name; + _parent_scene.actor_name_map[prim_geom] = this; + + } + else + m_log.Warn("Setting bad Geom"); + } + + + /// + /// Create a geometry for the given mesh in the given target space. + /// + /// + /// If null, then a mesh is used that is based on the profile shape data. + private void CreateGeom() + { + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + + bool haveMesh = false; + hasOOBoffsetFromMesh = false; + m_NoColide = false; + + if (_parent_scene.needsMeshing(_pbs)) + { + haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + if (!haveMesh) + m_NoColide = true; + } + + if (!haveMesh) + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return; + } + } + else + {// do it as a box + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + //Console.WriteLine(" CreateGeom 4"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + } + } + + /// + /// Set a new geometry for this prim. + /// + /// + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(prim_geom); + _parent_scene.actor_name_map.Remove(prim_geom); + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + // catch (System.AccessViolationException) + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); + } + + prim_geom = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + Body = IntPtr.Zero; + hasOOBoffsetFromMesh = false; + CalcPrimBodyData(); + } + + private void ChildSetGeom(OdePrim odePrim) + { + // well.. + DestroyBody(); + MakeBody(); + } + + //sets non physical prim m_targetSpace to right space in spaces grid for static prims + // should only be called for non physical prims unless they are becoming non physical + private void SetInStaticSpace(OdePrim prim) + { + IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); + prim.m_targetSpace = targetSpace; + d.GeomEnable(prim_geom); + } + + public void enableBodySoft() + { + if (!childPrim && !m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prm.prim_geom); + } + } + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prim_geom); + } + d.BodyEnable(Body); + } + } + m_disabled = false; + resetCollisionAccounting(); // this sets m_disable to false + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero) + { + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prm.prim_geom); + } + } + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prim_geom); + } + + d.BodyDisable(Body); + } + } + } + + private void MakeBody() + { + if (!m_isphysical) // only physical get bodies + return; + + if (childPrim) // child prims don't get bodies; + return; + + if (m_building) + return; + + if (prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); + return; + } + + if (Body != IntPtr.Zero) + { + d.BodyDestroy(Body); + Body = IntPtr.Zero; + m_log.Warn("[PHYSICS]: MakeBody called having a body"); + } + + + if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); + } + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + Body = d.BodyCreate(_parent_scene.world); + + DMassDup(ref primdMass, out objdmass); + + // rotate inertia + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + // set the body rotation and position + d.BodySetRotation(Body, ref mymat); + + // recompute full object inertia if needed + if (childrenPrim.Count > 0) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; + Vector3 rcm; + + rcm.X = _position.X + objdmass.c.X; + rcm.Y = _position.Y + objdmass.c.Y; + rcm.Z = _position.Z + objdmass.c.Z; + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); + continue; + } + + DMassCopy(ref prm.primdMass, ref tmpdmass); + + // apply prim current rotation to inertia + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + quat.W = prm._orientation.W; + d.RfromQ(out mat, ref quat); + d.MassRotate(ref tmpdmass, ref mat); + + Vector3 ppos = prm._position; + ppos.X += tmpdmass.c.X - rcm.X; + ppos.Y += tmpdmass.c.Y - rcm.Y; + ppos.Z += tmpdmass.c.Z - rcm.Z; + + // refer inertia to root prim center of mass position + d.MassTranslate(ref tmpdmass, + ppos.X, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats + + if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); + } + + d.GeomClearOffset(prm.prim_geom); + d.GeomSetBody(prm.prim_geom, Body); + prm.Body = Body; + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.W = -myrot.W; + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + // d.BodySetLinearDampingThreshold(Body, 0.01f); + // d.BodySetAngularDampingThreshold(Body, 0.001f); + d.BodySetDamping(Body, .002f, .002f); + + + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } + + + if (childrenPrim.Count == 0) + { + collide_geom = prim_geom; + m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prim_geom); + } + else + { + m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); + d.HashSpaceSetLevels(m_targetSpace, -2, 8); + d.SpaceSetSublevel(m_targetSpace, 3); + d.SpaceSetCleanup(m_targetSpace, false); + d.SpaceAdd(m_targetSpace, prim_geom); + collide_geom = m_targetSpace; + } + + if (m_delaySelect) + { + m_isSelected = true; + m_delaySelect = false; + } + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + continue; + + Vector3 ppos = prm._position; + d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position + + if (prm.m_targetSpace != m_targetSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = m_targetSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } + + if (m_isSelected || m_disabled) + { + prm.m_collisionCategories &= ~CollisionCategories.Body; + prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); + d.GeomDisable(prm.prim_geom); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = CollisionCategories.Land; + } + else + { + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + d.GeomEnable(prm.prim_geom); + } + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + d.GeomEnable(prm.prim_geom); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + } + prm.m_collisionscore = 0; + + if(!m_disabled) + prm.m_disabled = false; + + _parent_scene.addActivePrim(prm); + } + } + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + + if (m_isSelected || m_disabled) + { + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); + + d.GeomDisable(prim_geom); + d.BodyDisable(Body); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + m_collisionscore = 0; + + m_softcolide = true; + _parent_scene.addActivePrim(this); + _parent_scene.addActiveGroups(this); + } + + private void DestroyBody() + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + + if (!childPrim) + { + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); + } + prm.Body = IntPtr.Zero; + prm._mass = prm.primMass; + prm.m_collisionscore = 0; + } + } + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + _parent_scene.remActiveGroup(this); + d.BodyDestroy(Body); + } + Body = IntPtr.Zero; + } + _mass = primMass; + m_collisionscore = 0; + } + + #region Mass Calculation + + private float CalculatePrimVolume() + { + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.52359877559829887307710723054658f; + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + return volume; + } + + + private void CalcPrimBodyData() + { + float volume; + + if (prim_geom == IntPtr.Zero) + { + // Ubit let's have a initial basic OOB + primOOBsize.X = _size.X; + primOOBsize.Y = _size.Y; + primOOBsize.Z = _size.Z; + primOOBoffset = Vector3.Zero; + } + else + { + d.AABB AABB; + d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom + + primOOBsize.X = (AABB.MaxX - AABB.MinX); + primOOBsize.Y = (AABB.MaxY - AABB.MinY); + primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); + if (!hasOOBoffsetFromMesh) + { + primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; + primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; + primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; + } + } + + // also its own inertia and mass + // keep using basic shape mass for now + volume = CalculatePrimVolume(); + + primMass = m_density * volume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + _mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); + + d.MassTranslate(ref primdMass, + primOOBoffset.X, + primOOBoffset.Y, + primOOBoffset.Z); + + primOOBsize *= 0.5f; // let obb size be a corner coords + primOOBradiusSQ = primOOBsize.LengthSquared(); + } + + + #endregion + + + /// + /// Add a child prim to this parent prim. + /// + /// Child prim + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) + { + //Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) + { + DestroyBody(); // for now we need to rebuil entire object on link change + + lock (childrenPrim) + { + // adopt the prim + if (!childrenPrim.Contains(prim)) + childrenPrim.Add(prim); + + // see if this prim has kids and adopt them also + // should not happen for now + foreach (OdePrim prm in prim.childrenPrim) + { + if (!childrenPrim.Contains(prm)) + { + if (prm.Body != IntPtr.Zero) + { + if (prm.prim_geom != IntPtr.Zero) + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + if (prm.Body != prim.Body) + prm.DestroyBody(); // don't loose bodies around + prm.Body = IntPtr.Zero; + } + + childrenPrim.Add(prm); + prm._parent = this; + } + } + } + //Remove old children from the prim + prim.childrenPrim.Clear(); + + if (prim.Body != IntPtr.Zero) + { + if (prim.prim_geom != IntPtr.Zero) + d.GeomSetBody(prim.prim_geom, IntPtr.Zero); + prim.DestroyBody(); // don't loose bodies around + prim.Body = IntPtr.Zero; + } + + prim.childPrim = true; + prim._parent = this; + + MakeBody(); // full nasty reconstruction + } + } + + private void UpdateChildsfromgeom() + { + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.UpdateDataFromGeom(); + } + } + + private void UpdateDataFromGeom() + { + if (prim_geom != IntPtr.Zero) + { + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + d.Quaternion qtmp = new d.Quaternion { }; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.W = qtmp.W; + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + } + } + + private void ChildDelink(OdePrim odePrim, bool remakebodies) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) // delinking the root prim + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + if (remakebodies) + newroot.MakeBody(); + } + } + } + + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + // odePrim.UpdateDataFromGeom(); + if (remakebodies) + odePrim.MakeBody(); + } + } + if (remakebodies) + MakeBody(); + } + + protected void ChildRemove(OdePrim odePrim, bool reMakeBody) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + newroot.MakeBody(); + } + } + if (reMakeBody) + MakeBody(); + return; + } + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + if (reMakeBody) + odePrim.MakeBody(); + } + } + MakeBody(); + } + + #region changes + + private void changeadd() + { + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + + if (!m_isphysical) + SetInStaticSpace(this); + } + + if (m_isphysical && Body == IntPtr.Zero) + { + MakeBody(); + } + } + + private void changeAngularLock(Vector3 newLock) + { + // do we have a Physical object? + if (Body != IntPtr.Zero) + { + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) + { + if (!newLock.ApproxEquals(Vector3.One, 0f)) + { + createAMotor(newLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = newLock; + } + + private void changeLink(OdePrim NewParent) + { + if (_parent == null && NewParent != null) + { + NewParent.ParentPrim(this); + } + else if (_parent != null) + { + if (_parent is OdePrim) + { + if (NewParent != _parent) + { + (_parent as OdePrim).ChildDelink(this, false); // for now... + childPrim = false; + + if (NewParent != null) + { + NewParent.ParentPrim(this); + } + } + } + } + _parent = NewParent; + } + + + private void Stop() + { + if (!childPrim) + { + m_force = Vector3.Zero; + m_forceacc = Vector3.Zero; + m_angularForceacc = Vector3.Zero; + _torque = Vector3.Zero; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + } + + if (Body != IntPtr.Zero) + { + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetTorque(Body, 0f, 0f, 0f); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetAngularVel(Body, 0f, 0f, 0f); + } + } + + + private void changePhantomStatus(bool newval) + { + m_isphantom = newval; + + if (m_isSelected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; // should never happen + } + + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } + } + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + if(!m_isSelected) + d.GeomEnable(prm.prim_geom); + } + } + } + + if (!m_disabled && prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + if(!m_isSelected) + d.GeomEnable(prim_geom); + } + } + + private void changeSelectedStatus(bool newval) + { + if (m_lastdoneSelected == newval) + return; + + m_lastdoneSelected = newval; + DoSelectedStatus(newval); + } + + private void CheckDelaySelect() + { + if (m_delaySelect) + { + DoSelectedStatus(m_isSelected); + } + } + + private void DoSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); + + if (newval) + { + if (!childPrim && Body != IntPtr.Zero) + d.BodyDisable(Body); + + if (m_delaySelect || m_isphysical) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != null) + { + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prm.prim_geom); + } + prm.m_delaySelect = false; + } + } + + if (prim_geom != null) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prim_geom); + } + + m_delaySelect = false; + } + else if(!m_isphysical) + { + m_delaySelect = true; + } + } + else + { + if (!childPrim && Body != IntPtr.Zero && !m_disabled) + d.BodyEnable(Body); + + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if(m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prm.prim_geom); + } + prm.m_delaySelect = false; + prm.m_softcolide = true; + } + } + + if (!m_disabled && prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prim_geom); + } + + m_delaySelect = false; + m_softcolide = true; + } + + resetCollisionAccounting(); + } + + private void changePosition(Vector3 newPos) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _position = newPos; + } + } + else + { + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + // changeSelectedStatus(); + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changeOrientation(Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _orientation = newOri; + } + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + } + } + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim && m_building) // inertia is messed, must rebuild + { + _position = newPos; + _orientation = newOri; + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + + m_softcolide = true; + resetCollisionAccounting(); + } + + + private void changeDisable(bool disable) + { + if (disable) + { + if (!m_disabled) + disableBodySoft(); + } + else + { + if (m_disabled) + enableBodySoft(); + } + } + + private void changePhysicsStatus(bool NewStatus) + { + CheckDelaySelect(); + + m_isphysical = NewStatus; + + if (!childPrim) + { + if (NewStatus) + { + if (Body == IntPtr.Zero) + MakeBody(); + } + else + { + if (Body != IntPtr.Zero) + { + DestroyBody(); + } + Stop(); + } + } + + resetCollisionAccounting(); + } + + private void changeprimsizeshape() + { + CheckDelaySelect(); + + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + // we don't need to do space calculation because the client sends a position update also. + if (_size.X <= 0) + _size.X = 0.01f; + if (_size.Y <= 0) + _size.Y = 0.01f; + if (_size.Z <= 0) + _size.Z = 0.01f; + // Construction of new prim + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changeSize(Vector3 newSize) + { + _size = newSize; + changeprimsizeshape(); + } + + private void changeShape(PrimitiveBaseShape newShape) + { + if(newShape != null) + _pbs = newShape; + changeprimsizeshape(); + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; + + if (prim_geom != IntPtr.Zero && !m_isphantom) + { + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } + else + { + m_collisionFlags &= ~CollisionCategories.Water; + } + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } + + private void changeSetTorque(Vector3 newtorque) + { + if (!m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + } + _torque = newtorque; + } + } + + private void changeForce(Vector3 force) + { + m_force = force; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeAddForce(Vector3 force) + { + m_forceacc += force; + if (!m_isSelected) + { + lock (this) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + + m_collisionscore = 0; + } + } + + private void changeAddAngularForce(Vector3 aforce) + { + m_angularForceacc += aforce; + if (!m_isSelected) + { + lock (this) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + m_collisionscore = 0; + } + } + + private void changevelocity(Vector3 newVel) + { + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; + } + + private void changeVolumedetetion(bool newVolDtc) + { + m_isVolumeDetect = newVolDtc; + } + + protected void changeBuilding(bool newbuilding) + { + if ((bool)newbuilding) + { + m_building = true; + if (!childPrim) + DestroyBody(); + } + else + { + m_building = false; + CheckDelaySelect(); + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } + } + + public void changeSetVehicle(VehicleData vdata) + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + m_vehicle.DoSetVehicle(vdata); + } + private void changeVehicleType(int value) + { + if (value == (int)Vehicle.TYPE_NONE) + { + if (m_vehicle != null) + m_vehicle = null; + } + else + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + + private void changeVehicleFloatParam(strVehicleFloatParam fp) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + } + + private void changeVehicleVectorParam(strVehicleVectorParam vp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + } + + private void changeVehicleRotationParam(strVehicleQuatParam qp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); + } + + private void changeVehicleFlags(strVehicleBoolParam bp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + } + + #endregion + + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) + // !m_disabled && !m_isSelected && !m_building && !m_outbounds) + { +// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + float timestep = _parent_scene.ODE_STEPSIZE; + + // check outside region + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; + + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if(m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } + + + float fx = 0; + float fy = 0; + float fz = 0; + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + } + else + { + float m_mass = _mass; + + // fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + if (m_usePID) + { + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID) + { + //Console.WriteLine("Hover " + Name); + + // If we're using the PID controller, then we have no gravity + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } + else + { + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } + + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; + + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; + + m_forceacc = Vector3.Zero; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + + Vector3 trq; + + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } + + } + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; + //Console.WriteLine("Nothing " + Name); + + } + } + + + public void UpdatePositionAndVelocity(float simulatedtime) + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null && !m_disabled && !m_building && !m_outbounds) + { + if (Body != IntPtr.Zero) + { + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + + if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) + && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) + && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) + && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) + && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) + && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) + ) + { + _zeroFlag = true; + //Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } + + if (_zeroFlag) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastVelocity = _velocity; + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + + _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + { + m_rotationalVelocity = pv; + } + else + { + m_rotationalVelocity.X = rotvel.X; + m_rotationalVelocity.Y = rotvel.Y; + m_rotationalVelocity.Z = rotvel.Z; + } + + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + m_lastposition = _position; + m_lastorientation = _orientation; + base.RequestPhysicsterseUpdate(); + } + else + { + throttleCounter++; + } + } + } + else if (!m_lastUpdateSent || !_zeroFlag) + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + } + } + + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } + + internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) + { + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + + private static void DMassDup(ref d.Mass src, out d.Mass dst) + { + dst = new d.Mass { }; + + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; + case changes.Remove: + //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... + //When we return true, it destroys all of the prims in the linkset anyway + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildRemove(this, false); + } + else + ChildRemove(this, false); + + m_vehicle = null; + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + if (m_eventsubscription > 0) + UnSubscribeEvents(); + return true; + + case changes.Link: + OdePrim tmp = (OdePrim)arg; + changeLink(tmp); + break; + + case changes.DeLink: + changeLink(null); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changevelocity((Vector3)arg); + break; + + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Phantom: + changePhantomStatus((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; + + case changes.VehicleType: + changeVehicleType((int)arg); + break; + + case changes.VehicleFlags: + changeVehicleFlags((strVehicleBoolParam) arg); + break; + + case changes.VehicleFloatParam: + changeVehicleFloatParam((strVehicleFloatParam) arg); + break; + + case changes.VehicleVectorParam: + changeVehicleVectorParam((strVehicleVectorParam) arg); + break; + + case changes.VehicleRotationParam: + changeVehicleRotationParam((strVehicleQuatParam) arg); + break; + + case changes.SetVehicle: + changeSetVehicle((VehicleData) arg); + break; + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor) this, what, arg); + } + + + private struct strVehicleBoolParam + { + public int param; + public bool value; + } + + private struct strVehicleFloatParam + { + public int param; + public float value; + } + + private struct strVehicleQuatParam + { + public int param; + public Quaternion value; + } + + private struct strVehicleVectorParam + { + public int param; + public Vector3 value; + } + } } \ No newline at end of file -- cgit v1.1 From 316f272b621741e694f2b522910594a0e5879634 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 21 Mar 2012 12:57:46 +0000 Subject: shapetype support on chOde so it can also request a simple convex hull 'mesh' --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 36 ++++++++++++++++++-- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 44 +++++++------------------ 2 files changed, 46 insertions(+), 34 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index f9548d2..4cf88d8 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -179,6 +179,9 @@ namespace OpenSim.Region.Physics.OdePlugin public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; + private byte m_shapetype; + private byte m_taintshapetype; + public bool _zeroFlag; // if body has been stopped private bool m_lastUpdateSent; @@ -315,7 +318,8 @@ namespace OpenSim.Region.Physics.OdePlugin } public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom, CollisionLocker dode, uint localid) + Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, + bool pisPhantom,byte shapetype, CollisionLocker dode, uint localid) { m_localID = localid; ode = dode; @@ -360,6 +364,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintrot = _orientation; _mesh = mesh; _pbs = pbs; + m_shapetype = shapetype; + m_taintshapetype = shapetype; _parent_scene = parent_scene; m_targetSpace = (IntPtr)0; @@ -605,6 +611,19 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public override byte PhysicsShapeType + { + get + { + return m_shapetype; + } + set + { + m_taintshapetype = value; + _parent_scene.AddPhysicsActorTaint(this); + } + } + public override Vector3 Velocity { get @@ -1535,6 +1554,12 @@ namespace OpenSim.Region.Physics.OdePlugin changesize(timestep); // + if(m_taintshapetype != m_shapetype) + { + m_shapetype = m_taintshapetype; + changeshape(timestep); + } + if (m_taintshape) changeshape(timestep); // @@ -2476,9 +2501,16 @@ namespace OpenSim.Region.Physics.OdePlugin if (IsPhysical) meshlod = _parent_scene.MeshSculptphysicalLOD; + + bool convex; + if (m_shapetype == 2) + convex = true; + else + convex = false; + try { - mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true); + mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, convex); } catch { diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index ea89d87..00f5122 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1720,7 +1720,7 @@ namespace OpenSim.Region.Physics.OdePlugin } private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, uint localid) + IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, bool isphantom, byte shapetype, uint localid) { Vector3 pos = position; @@ -1730,27 +1730,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical,false, ode, localid); - - lock (_prims) - _prims.Add(newPrim); - } - - return newPrim; - } - - private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, bool isphantom, uint localid) - { - - Vector3 pos = position; - Vector3 siz = size; - Quaternion rot = rotation; - - OdePrim newPrim; - lock (OdeLock) - { - newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, isphantom, ode, localid); + newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, isphantom, shapetype, ode, localid); lock (_prims) _prims.Add(newPrim); @@ -1781,7 +1761,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (needsMeshing(pbs)) mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true); - result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, localid); + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical,false,0, localid); return result; } @@ -1795,25 +1775,25 @@ namespace OpenSim.Region.Physics.OdePlugin if (needsMeshing(pbs)) mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true); - result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, isPhantom, localid); + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, isPhantom,0, localid); return result; } - - -/* - public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, - uint localid, byte[] sdata) + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid) { PhysicsActor result; + IMesh mesh = null; - result = AddPrim(primName, position, parent, - pbs, localid, sdata); + if (needsMeshing(pbs)) + mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true); + + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, isPhantom, shapetype, localid); return result; } -*/ + public override float TimeDilation { get { return m_timeDilation; } -- cgit v1.1 From a03c55fee7f07053c699ddf519496caac9d1df6f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 21 Mar 2012 14:03:50 +0000 Subject: missed a creatMesh in chODE. temporary removed m_meshfailed test since it may colide with how meshs and sculpts are loaded. This needs a good revision.. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 4cf88d8..3e2b71c 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -2137,13 +2137,18 @@ namespace OpenSim.Region.Physics.OdePlugin m_targetSpace = targetspace; - if (_mesh == null && m_meshfailed == false) + if (_mesh == null) // && m_meshfailed == false) { if (_parent_scene.needsMeshing(_pbs)) { + bool convex; + if (m_shapetype == 2) + convex = true; + else + convex = false; try { - _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true); + _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,convex); } catch { @@ -2382,7 +2387,7 @@ namespace OpenSim.Region.Physics.OdePlugin // we don't need to do space calculation because the client sends a position update also. // Construction of new prim - if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false) + if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false) { float meshlod = _parent_scene.meshSculptLOD; @@ -2493,7 +2498,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (_size.Z <= 0) _size.Z = 0.01f; // Construction of new prim - if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false) + if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false) { // Don't need to re-enable body.. it's done in SetMesh float meshlod = _parent_scene.meshSculptLOD; -- cgit v1.1 From 4f593fa8c3733cfccc69782bfa3fbd91543a146b Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 21 Mar 2012 21:17:00 +0000 Subject: minor clean --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 51 +++++++++++------------ 1 file changed, 24 insertions(+), 27 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index df08381..8e903e8 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -218,9 +218,20 @@ namespace OpenSim.Region.Physics.Meshing // geometry for this submesh. if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"])) return; - - OpenMetaverse.Vector3 posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3(); - OpenMetaverse.Vector3 posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3(); + + OpenMetaverse.Vector3 posMax; + OpenMetaverse.Vector3 posMin; + if (subMeshData.ContainsKey("PositionDomain")) + { + posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3(); + posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3(); + } + else + { + posMax = new Vector3(0.5f, 0.5f, 0.5f); + posMin = new Vector3(-0.5f, -0.5f, -0.5f); + } + ushort faceIndexOffset = (ushort)coords.Count; byte[] posBytes = subMeshData["Position"].AsBinary(); @@ -280,18 +291,14 @@ namespace OpenSim.Region.Physics.Meshing { if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) return null; - // Remove the reference to any JPEG2000 sculpt data so it can be GCed - // don't loose it - // primShape.SculptData = Utils.EmptyBytes; } -// primShape.SculptDataLoaded = true; } else { if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) return null; } - // keep compatible + primShape.SculptData = Utils.EmptyBytes; int numCoords = coords.Count; @@ -307,10 +314,6 @@ namespace OpenSim.Region.Physics.Meshing coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z)); } - -// mesh.DumpRaw("c:\\lixo", "lixo", "lixo"); - mesh.DumpRaw(".", "lixo", "lixo"); - return mesh; } @@ -452,22 +455,20 @@ namespace OpenSim.Region.Physics.Meshing return false; byte[] data; - const float invMaxU16 = 1.0f / 65535f; - int t1; - int t2; - int t3; - int i; List vs = new List(); - - float3 f3; PHullResult hullr = new PHullResult(); - + float3 f3; Coord c; Face f; - Vector3 range; Vector3 min; + + const float invMaxU16 = 1.0f / 65535f; + int t1; + int t2; + int t3; + int i; int nverts; int nindexs; @@ -576,8 +577,7 @@ namespace OpenSim.Region.Physics.Meshing c.Z = hullr.Vertices[i].z; coords.Add(c); } - - + for (i = 0; i < nindexs; i += 3) { t1 = hullr.Indices[i]; @@ -597,8 +597,7 @@ namespace OpenSim.Region.Physics.Meshing } } if (coords.Count > 0 && faces.Count > 0) - return true; - + return true; } vs.Clear(); @@ -642,7 +641,6 @@ namespace OpenSim.Region.Physics.Meshing if (vs.Count == 4) { - // not sure about orientation.. f = new Face(0, 2, 3); faces.Add(f); f = new Face(0, 3, 1); @@ -690,7 +688,6 @@ namespace OpenSim.Region.Physics.Meshing } else return false; - } } -- cgit v1.1 From 21a97408d4b209f22dabbe1203cffc388d9757bf Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 24 Mar 2012 11:30:29 +0000 Subject: Avatars have no bounce --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 4 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 7645 ++++++++++---------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 11 +- 3 files changed, 3955 insertions(+), 3705 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 9a22331..4266fda 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -137,7 +137,6 @@ namespace OpenSim.Region.Physics.OdePlugin public bool bad = false; float mu; - float bounce; public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) { @@ -170,7 +169,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_mass = 80f; // sure we have a default mu = parent_scene.AvatarFriction; - bounce = parent_scene.AvatarBounce; walkDivisor = walk_divisor; runDivisor = rundivisor; @@ -194,7 +192,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override void getContactData(ref ContactData cdata) { cdata.mu = mu; - cdata.bounce = bounce; + cdata.bounce = 0; cdata.softcolide = false; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 71aec4c..bacd604 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1,3725 +1,3976 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -/* Revision 2011/12 by Ubit Umarov - * - * - */ - -/* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - */ - -//#define SPAM - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Threading; -using log4net; -using OpenMetaverse; -using OdeAPI; -using OpenSim.Framework; -using OpenSim.Region.Physics.Manager; - - -namespace OpenSim.Region.Physics.OdePlugin -{ - public class OdePrim : PhysicsActor - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - private bool m_isphysical; - private bool m_fakeisphysical; - private bool m_isphantom; +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* Revision 2011/12 by Ubit Umarov + * + * + */ + +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_isphysical; + private bool m_fakeisphysical; + private bool m_isphantom; private bool m_fakeisphantom; protected bool m_building; - private Quaternion m_lastorientation = new Quaternion(); - private Quaternion _orientation; - - private Vector3 _position; - private Vector3 _velocity; - private Vector3 _torque; - private Vector3 m_lastVelocity; - private Vector3 m_lastposition; - private Vector3 m_rotationalVelocity; - private Vector3 _size; - private Vector3 _acceleration; - private Vector3 m_angularlock = Vector3.One; - private IntPtr Amotor = IntPtr.Zero; - - private Vector3 m_force; - private Vector3 m_forceacc; - private Vector3 m_angularForceacc; - - private Vector3 m_PIDTarget; - private float m_PIDTau; - private float PID_D = 35f; - private float PID_G = 25f; - private bool m_usePID; - - // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), - // and are for non-VEHICLES only. - - private float m_PIDHoverHeight; - private float m_PIDHoverTau; - private bool m_useHoverPID; - private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; - private float m_targetHoverHeight; - private float m_groundHeight; - private float m_waterHeight; - private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - - private int body_autodisable_frames = 20; - - private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom - | CollisionCategories.Space - | CollisionCategories.Body - | CollisionCategories.Character - ); -// private bool m_collidesLand = true; - private bool m_collidesWater; - public bool m_returnCollisions; - private bool m_softcolide; - - private bool m_NoColide; // for now only for internal use for bad meshs - - // Default we're a Geometry - private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); - - // Default, Collide with Other Geometries, spaces and Bodies - private CollisionCategories m_collisionFlags = m_default_collisionFlags; - - public bool m_disabled; - - - public uint m_localID; - - private PrimitiveBaseShape _pbs; - public OdeScene _parent_scene; - - /// - /// The physics space which contains prim geometry - /// - public IntPtr m_targetSpace = IntPtr.Zero; - - public IntPtr prim_geom; - public IntPtr _triMeshData; - - private PhysicsActor _parent; - - private List childrenPrim = new List(); - - private bool m_iscolliding; - - public bool m_isSelected; - private bool m_delaySelect; - private bool m_lastdoneSelected; - public bool m_outbounds; - - internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - - private bool m_throttleUpdates; - private int throttleCounter; - public float m_collisionscore; - int m_colliderfilter = 0; - - public IntPtr collide_geom; // for objects: geom if single prim space it linkset - - private float m_density = 10.000006836f; // Aluminum g/cm3; - private byte m_shapetype; - public bool _zeroFlag; - private bool m_lastUpdateSent; - - public IntPtr Body = IntPtr.Zero; - public String Name { get; private set; } - private Vector3 _target_velocity; - - public Vector3 primOOBsize; // prim real dimensions from mesh - public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb - public float primOOBradiusSQ; - public d.Mass primdMass; // prim inertia information on it's own referencial - float primMass; // prim own mass - float _mass; // object mass acording to case - private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb - - public int givefakepos = 0; - private Vector3 fakepos; - public int givefakeori = 0; - private Quaternion fakeori; - - public int m_eventsubscription; - private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); - - public volatile bool childPrim; - - public ODEDynamics m_vehicle; - - internal int m_material = (int)Material.Wood; - private float mu; - private float bounce; - - /// - /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. - /// - public override bool IsPhysical // this is not reliable for internal use - { - get { return m_fakeisphysical; } - set - { - m_fakeisphysical = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - - if (!value) // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - AddChange(changes.Physical, value); - } - } - - public override bool Phantom // this is not reliable for internal use - { - get { return m_fakeisphantom; } - set - { - m_fakeisphantom = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - - AddChange(changes.Phantom, value); - } - } - - public override bool Building // this is not reliable for internal use - { - get { return m_building; } - set - { - if (value) - m_building = true; - AddChange(changes.building, value); - } - } - - public override void getContactData(ref ContactData cdata) - { - cdata.mu = mu; - cdata.bounce = bounce; - - // cdata.softcolide = m_softcolide; - cdata.softcolide = false; - - if (m_isphysical) - { - ODEDynamics veh; - if (_parent != null) - veh = ((OdePrim)_parent).m_vehicle; - else - veh = m_vehicle; - - if (veh != null && veh.Type != Vehicle.TYPE_NONE) - cdata.mu *= veh.FrictionFactor; - } - } - - public override int PhysicsActorType - { - get { return (int)ActorTypes.Prim; } - set { return; } - } - - public override bool SetAlwaysRun - { - get { return false; } - set { return; } - } - - public override uint LocalID - { - get - { - return m_localID; - } - set - { - //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); - m_localID = value; - } - } - - public override bool Grabbed - { - set { return; } - } - - public override bool Selected - { - set - { - if (value) - m_isSelected = value; // if true set imediatly to stop moves etc - AddChange(changes.Selected, value); - } - } - - public override bool Flying - { - // no flying prims for you - get { return false; } - set { } - } - - public override bool IsColliding - { - get { return m_iscolliding; } - set - { - if (value) - { - m_colliderfilter += 2; - if (m_colliderfilter > 2) - m_colliderfilter = 2; - } - else - { - m_colliderfilter--; - if (m_colliderfilter < 0) - m_colliderfilter = 0; - } - - if (m_colliderfilter == 0) - { - m_softcolide = false; - m_iscolliding = false; - } - else - m_iscolliding = true; - } - } - - public override bool CollidingGround - { - get { return false; } - set { return; } - } - - public override bool CollidingObj - { - get { return false; } - set { return; } - } - - public override bool ThrottleUpdates - { - get { return m_throttleUpdates; } - set { m_throttleUpdates = value; } - } - - public override bool Stopped - { - get { return _zeroFlag; } - } - - public override Vector3 Position - { - get - { - if (givefakepos > 0) - return fakepos; - else - return _position; - } - - set - { - fakepos = value; - givefakepos++; - AddChange(changes.Position, value); - } - } - - public override Vector3 Size - { - get { return _size; } - set - { - if (value.IsFinite()) - { - AddChange(changes.Size, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); - } - } - } - - public override float Mass - { - get { return _mass; } - } - - public override Vector3 Force - { - //get { return Vector3.Zero; } - get { return m_force; } - set - { - if (value.IsFinite()) - { - AddChange(changes.Force, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); - } - } - } - - public override void SetVolumeDetect(int param) - { - AddChange(changes.VolumeDtc, (param != 0)); - } - - public override Vector3 GeometricCenter - { - get - { - return Vector3.Zero; - } - } - - public override Vector3 CenterOfMass - { - get - { - d.Vector3 dtmp; - if (IsPhysical && !childPrim && Body != IntPtr.Zero) - { - dtmp = d.BodyGetPosition(Body); - return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - } - else if (prim_geom != IntPtr.Zero) - { - d.Quaternion dq; - d.GeomCopyQuaternion(prim_geom, out dq); - Quaternion q; - q.X = dq.X; - q.Y = dq.Y; - q.Z = dq.Z; - q.W = dq.W; - - Vector3 vtmp = primOOBoffset * q; - dtmp = d.GeomGetPosition(prim_geom); - return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); - } - else - return Vector3.Zero; - } - } - /* - public override Vector3 PrimOOBsize - { - get - { - return primOOBsize; - } - } - - public override Vector3 PrimOOBoffset - { - get - { - return primOOBoffset; - } - } - - public override float PrimOOBRadiusSQ - { - get - { - return primOOBradiusSQ; - } - } - */ - public override PrimitiveBaseShape Shape - { - set - { - AddChange(changes.Shape, value); - } - } - - public override byte PhysicsShapeType - { - get - { - return m_shapetype; - } - set - { - m_shapetype = value; - AddChange(changes.Shape, null); - } - } - - public override Vector3 Velocity - { - get - { - if (_zeroFlag) - return Vector3.Zero; - return _velocity; - } - set - { - if (value.IsFinite()) - { - AddChange(changes.Velocity, value); - // _velocity = value; - - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); - } - - } - } - - public override Vector3 Torque - { - get - { - if (!IsPhysical || Body == IntPtr.Zero) - return Vector3.Zero; - - return _torque; - } - - set - { - if (value.IsFinite()) - { - AddChange(changes.Torque, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); - } - } - } - - public override float CollisionScore - { - get { return m_collisionscore; } - set { m_collisionscore = value; } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override Quaternion Orientation - { - get - { - if (givefakeori > 0) - return fakeori; - else - - return _orientation; - } - set - { - if (QuaternionIsFinite(value)) - { - fakeori = value; - givefakeori++; - AddChange(changes.Orientation, value); - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); - - } - } - - public override Vector3 Acceleration - { - get { return _acceleration; } - set { } - } - - public override Vector3 RotationalVelocity - { - get - { - Vector3 pv = Vector3.Zero; - if (_zeroFlag) - return pv; - m_lastUpdateSent = false; - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - return pv; - - return m_rotationalVelocity; - } - set - { - if (value.IsFinite()) - { - m_rotationalVelocity = value; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); - } - } - } - + protected bool m_forcePosOrRotation; + + private Quaternion m_lastorientation = new Quaternion(); + private Quaternion _orientation; + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _torque; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 _acceleration; + private Vector3 m_angularlock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; + + private Vector3 m_force; + private Vector3 m_forceacc; + private Vector3 m_angularForceacc; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private float PID_D = 35f; + private float PID_G = 25f; + private bool m_usePID; + + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private float m_targetHoverHeight; + private float m_groundHeight; + private float m_waterHeight; + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + + private int body_autodisable_frames = 20; + + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); +// private bool m_collidesLand = true; + private bool m_collidesWater; + public bool m_returnCollisions; + private bool m_softcolide; + + private bool m_NoColide; // for now only for internal use for bad meshs + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlags; + + public bool m_disabled; + + + public uint m_localID; + + private PrimitiveBaseShape _pbs; + public OdeScene _parent_scene; + + /// + /// The physics space which contains prim geometry + /// + public IntPtr m_targetSpace = IntPtr.Zero; + + public IntPtr prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent; + + private List childrenPrim = new List(); + + private bool m_iscolliding; + + public bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + public bool m_outbounds; + + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + private bool m_throttleUpdates; + private int throttleCounter; + public float m_collisionscore; + int m_colliderfilter = 0; + + public IntPtr collide_geom; // for objects: geom if single prim space it linkset + + private float m_density = 10.000006836f; // Aluminum g/cm3; + private byte m_shapetype; + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body = IntPtr.Zero; + public String Name { get; private set; } + private Vector3 _target_velocity; + + public Vector3 primOOBsize; // prim real dimensions from mesh + public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb + public float primOOBradiusSQ; + public d.Mass primdMass; // prim inertia information on it's own referencial + float primMass; // prim own mass + float _mass; // object mass acording to case + private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb + + public int givefakepos = 0; + private Vector3 fakepos; + public int givefakeori = 0; + private Quaternion fakeori; + + public int m_eventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + public volatile bool childPrim; + + public ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + private float mu; + private float bounce; + + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical // this is not reliable for internal use + { + get { return m_fakeisphysical; } + set + { + m_fakeisphysical = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + if (!value) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + AddChange(changes.Physical, value); + } + } + + public override bool Phantom // this is not reliable for internal use + { + get { return m_fakeisphantom; } + set + { + m_fakeisphantom = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + AddChange(changes.Phantom, value); + } + } + + public override bool Building // this is not reliable for internal use + { + get { return m_building; } + set + { + if (value) + m_building = true; + AddChange(changes.building, value); + } + } + + public override void getContactData(ref ContactData cdata) + { + cdata.mu = mu; + cdata.bounce = bounce; + + // cdata.softcolide = m_softcolide; + cdata.softcolide = false; + + if (m_isphysical) + { + ODEDynamics veh; + if (_parent != null) + veh = ((OdePrim)_parent).m_vehicle; + else + veh = m_vehicle; + + if (veh != null && veh.Type != Vehicle.TYPE_NONE) + cdata.mu *= veh.FrictionFactor; + } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Prim; } + set { return; } + } + + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + + public override uint LocalID + { + get + { + return m_localID; + } + set + { + //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); + m_localID = value; + } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set + { + if (value) + m_isSelected = value; // if true set imediatly to stop moves etc + AddChange(changes.Selected, value); + } + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + { + m_softcolide = false; + m_iscolliding = false; + } + else + m_iscolliding = true; + } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get + { + if (givefakepos > 0) + return fakepos; + else + return _position; + } + + set + { + fakepos = value; + givefakepos++; + AddChange(changes.Position, value); + } + } + + public override Vector3 Size + { + get { return _size; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Size, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + } + } + } + + public override float Mass + { + get { return _mass; } + } + + public override Vector3 Force + { + //get { return Vector3.Zero; } + get { return m_force; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Force, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + } + } + } + + public override void SetVolumeDetect(int param) + { + AddChange(changes.VolumeDtc, (param != 0)); + } + + public override Vector3 GeometricCenter + { + get + { + return Vector3.Zero; + } + } + + public override Vector3 CenterOfMass + { + get + { + d.Vector3 dtmp; + if (IsPhysical && !childPrim && Body != IntPtr.Zero) + { + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; + + Vector3 vtmp = primOOBoffset * q; + dtmp = d.GeomGetPosition(prim_geom); + return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); + } + else + return Vector3.Zero; + } + } + /* + public override Vector3 PrimOOBsize + { + get + { + return primOOBsize; + } + } + + public override Vector3 PrimOOBoffset + { + get + { + return primOOBoffset; + } + } + + public override float PrimOOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } + } + */ + public override PrimitiveBaseShape Shape + { + set + { + AddChange(changes.Shape, value); + } + } + + public override byte PhysicsShapeType + { + get + { + return m_shapetype; + } + set + { + m_shapetype = value; + AddChange(changes.Shape, null); + } + } + + + public override Vector3 Velocity + { + get + { + if (_zeroFlag) + return Vector3.Zero; + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); + // _velocity = value; + + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); + } + + } + } + + public override Vector3 Torque + { + get + { + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return _torque; + } + + set + { + if (value.IsFinite()) + { + AddChange(changes.Torque, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); + } + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get + { + if (givefakeori > 0) + return fakeori; + else + + return _orientation; + } + set + { + if (QuaternionIsFinite(value)) + { + fakeori = value; + givefakeori++; + AddChange(changes.Orientation, value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); + + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + m_buoyancy = value; + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + public override int VehicleType + { + // we may need to put a fake on this + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + AddChange(changes.VehicleType, value); + } + } + + public override void VehicleFloatParam(int param, float value) + { + strVehicleFloatParam fp = new strVehicleFloatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleFloatParam, fp); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + strVehicleVectorParam fp = new strVehicleVectorParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); + } + + public override void VehicleRotationParam(int param, Quaternion value) + { + strVehicleQuatParam fp = new strVehicleQuatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleRotationParam, fp); + } + + public override void VehicleFlags(int param, bool value) + { + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); + } + + public override void SetVehicle(object vdata) + { + AddChange(changes.SetVehicle, vdata); + } + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + if (m_outbounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + m_lastposition = _position; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + if(Body != IntPtr.Zero) + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + if (prim_geom != IntPtr.Zero) + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + + m_outbounds = false; + changeDisable(false); + base.RequestPhysicsterseUpdate(); + } + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + mu = _parent_scene.m_materialContactsData[pMaterial].mu; + bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public void setPrimForRemoval() + { + AddChange(changes.Remove, null); + } + + public override void link(PhysicsActor obj) + { + AddChange(changes.Link, obj); + } + + public override void delink() + { + AddChange(changes.DeLink, null); + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axis); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + _parent_scene.RemoveCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) + CollisionEventsThisFrame = null; + else + CollisionEventsThisFrame = new CollisionEventUpdate(); + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) + { + Name = primName; + LocalID = plocalID; + + m_vehicle = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); + } + _position = pos; + givefakepos = 0; + + PID_D = parent_scene.bodyPIDD; + PID_G = parent_scene.bodyPIDG; + m_density = parent_scene.geomDefaultDensity; + // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; + Body = IntPtr.Zero; + + if (!size.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + } + + _orientation = rotation; + givefakeori = 0; + + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_colliderfilter = 0; + m_softcolide = true; + m_NoColide = false; + + hasOOBoffsetFromMesh = false; + _triMeshData = IntPtr.Zero; + + m_shapetype = _shapeType; + + m_lastdoneSelected = false; + m_isSelected = false; + m_delaySelect = false; + + m_isphantom = pisPhantom; + m_fakeisphantom = pisPhantom; + + mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + CalcPrimBodyData(); + + m_building = true; // control must set this to false when done + + AddChange(changes.Add, null); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + } + + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); + + if (axisnum <= 0) + return; + + // stop it + d.BodySetTorque(Body, 0, 0, 0); + d.BodySetAngularVel(Body, 0, 0, 0); + + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + + d.JointSetAMotorNumAxes(Amotor, axisnum); + + // get current orientation to lock + + d.Quaternion dcur = d.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; + Vector3 ax; + + int i = 0; + int j = 0; + if (axis.X == 0) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + // ODE should do this with axis relative to body 1 but seems to fail + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); + i++; + j = 256; // move to next axis set + } + + if (axis.Y == 0) + { + ax = (new Vector3(0, 1, 0)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + i++; + j += 256; + } + + if (axis.Z == 0) + { + ax = (new Vector3(0, 0, 1)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + } + } + + private bool setMesh(OdeScene parent_scene) + { + if (Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this, false); + } + } + else + { + DestroyBody(); + } + } + + bool convex; + if (m_shapetype == 0) + convex = false; + else + convex = true; + + IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex); + if (mesh == null) + { + m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); + return false; + } + + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", + Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); + mesh.releaseSourceMeshData(); + return false; + } + + primOOBoffset = mesh.GetCentroid(); + hasOOBoffsetFromMesh = true; + + mesh.releaseSourceMeshData(); + + IntPtr geo = IntPtr.Zero; + + try + { + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + return false; + } + + SetGeom(geo); + return true; + } + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + { + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + CalcPrimBodyData(); + + _parent_scene.geom_name_map[prim_geom] = Name; + _parent_scene.actor_name_map[prim_geom] = this; + + } + else + m_log.Warn("Setting bad Geom"); + } + + + /// + /// Create a geometry for the given mesh in the given target space. + /// + /// + /// If null, then a mesh is used that is based on the profile shape data. + private void CreateGeom() + { + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + + bool haveMesh = false; + hasOOBoffsetFromMesh = false; + m_NoColide = false; + + if (_parent_scene.needsMeshing(_pbs)) + { + haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + if (!haveMesh) + m_NoColide = true; + } + + if (!haveMesh) + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return; + } + } + else + {// do it as a box + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + //Console.WriteLine(" CreateGeom 4"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + } + } + + /// + /// Set a new geometry for this prim. + /// + /// + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(prim_geom); + _parent_scene.actor_name_map.Remove(prim_geom); + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + // catch (System.AccessViolationException) + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); + } + + prim_geom = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + Body = IntPtr.Zero; + hasOOBoffsetFromMesh = false; + CalcPrimBodyData(); + } + + private void ChildSetGeom(OdePrim odePrim) + { + // well.. + DestroyBody(); + MakeBody(); + } + + //sets non physical prim m_targetSpace to right space in spaces grid for static prims + // should only be called for non physical prims unless they are becoming non physical + private void SetInStaticSpace(OdePrim prim) + { + IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); + prim.m_targetSpace = targetSpace; + d.GeomEnable(prim_geom); + } + + public void enableBodySoft() + { + if (!childPrim && !m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prm.prim_geom); + } + } + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prim_geom); + } + d.BodyEnable(Body); + } + } + m_disabled = false; + resetCollisionAccounting(); // this sets m_disable to false + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero) + { + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prm.prim_geom); + } + } + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prim_geom); + } + + d.BodyDisable(Body); + } + } + } + + private void MakeBody() + { + if (!m_isphysical) // only physical get bodies + return; + + if (childPrim) // child prims don't get bodies; + return; + + if (m_building) + return; + + if (prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); + return; + } + + if (Body != IntPtr.Zero) + { + d.BodyDestroy(Body); + Body = IntPtr.Zero; + m_log.Warn("[PHYSICS]: MakeBody called having a body"); + } + + + if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); + } + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + Body = d.BodyCreate(_parent_scene.world); + + DMassDup(ref primdMass, out objdmass); + + // rotate inertia + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + // set the body rotation + d.BodySetRotation(Body, ref mymat); + + // recompute full object inertia if needed + if (childrenPrim.Count > 0) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; + Vector3 rcm; + + rcm.X = _position.X + objdmass.c.X; + rcm.Y = _position.Y + objdmass.c.Y; + rcm.Z = _position.Z + objdmass.c.Z; + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); + continue; + } + + DMassCopy(ref prm.primdMass, ref tmpdmass); + + // apply prim current rotation to inertia + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + quat.W = prm._orientation.W; + d.RfromQ(out mat, ref quat); + d.MassRotate(ref tmpdmass, ref mat); + + Vector3 ppos = prm._position; + ppos.X += tmpdmass.c.X - rcm.X; + ppos.Y += tmpdmass.c.Y - rcm.Y; + ppos.Z += tmpdmass.c.Z - rcm.Z; + + // refer inertia to root prim center of mass position + d.MassTranslate(ref tmpdmass, + ppos.X, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats + + if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); + } + + d.GeomClearOffset(prm.prim_geom); + d.GeomSetBody(prm.prim_geom, Body); + prm.Body = Body; + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.W = -myrot.W; + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + // d.BodySetLinearDampingThreshold(Body, 0.01f); + // d.BodySetAngularDampingThreshold(Body, 0.001f); + d.BodySetDamping(Body, .002f, .002f); + + + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } + + + if (childrenPrim.Count == 0) + { + collide_geom = prim_geom; + m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prim_geom); + } + else + { + m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); + d.HashSpaceSetLevels(m_targetSpace, -2, 8); + d.SpaceSetSublevel(m_targetSpace, 3); + d.SpaceSetCleanup(m_targetSpace, false); + d.SpaceAdd(m_targetSpace, prim_geom); + collide_geom = m_targetSpace; + } + + if (m_delaySelect) + { + m_isSelected = true; + m_delaySelect = false; + } + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + continue; + + Vector3 ppos = prm._position; + d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position + + if (prm.m_targetSpace != m_targetSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = m_targetSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } + + if (m_isSelected || m_disabled) + { + prm.m_collisionCategories &= ~CollisionCategories.Body; + prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); + d.GeomDisable(prm.prim_geom); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = CollisionCategories.Land; + } + else + { + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + d.GeomEnable(prm.prim_geom); + } + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + d.GeomEnable(prm.prim_geom); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + } + prm.m_collisionscore = 0; + + if(!m_disabled) + prm.m_disabled = false; + + _parent_scene.addActivePrim(prm); + } + } + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + + if (m_isSelected || m_disabled) + { + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); + + d.GeomDisable(prim_geom); + d.BodyDisable(Body); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + m_collisionscore = 0; + + m_softcolide = true; + _parent_scene.addActivePrim(this); + _parent_scene.addActiveGroups(this); + } + + private void DestroyBody() + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + + if (!childPrim) + { + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); + } + prm.Body = IntPtr.Zero; + prm._mass = prm.primMass; + prm.m_collisionscore = 0; + } + } + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + _parent_scene.remActiveGroup(this); + d.BodyDestroy(Body); + } + Body = IntPtr.Zero; + } + _mass = primMass; + m_collisionscore = 0; + } + + private void FixInertia(Vector3 NewPos,Quaternion newrot) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + + d.BodyGetMass(Body, out tmpdmass); + objdmass = tmpdmass; + + d.Vector3 dobjpos; + d.Vector3 thispos; + + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + // get prim own inertia in its local frame + tmpdmass = primdMass; + + // transform to object frame + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // back prim own inertia + tmpdmass = primdMass; + + // update to new position and orientation + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } + + + + private void FixInertia(Vector3 NewPos) + { + d.Matrix3 primmat = new d.Matrix3(); + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Mass primmass = new d.Mass { }; - public override float Buoyancy - { - get { return m_buoyancy; } - set - { - m_buoyancy = value; - } - } + d.Vector3 dobjpos; + d.Vector3 thispos; - public override bool FloatOnWater - { - set - { - AddChange(changes.CollidesWater, value); - } - } + d.BodyGetMass(Body, out objdmass); - public override Vector3 PIDTarget - { - set - { - if (value.IsFinite()) - { - m_PIDTarget = value; - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); - } - } + // get prim own inertia in its local frame + primmass = primdMass; + // transform to object frame + primmat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref primmass, ref primmat); - public override bool PIDActive { set { m_usePID = value; } } - public override float PIDTau { set { m_PIDTau = value; } } + tmpdmass = primmass; - public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } - public override bool PIDHoverActive { set { m_useHoverPID = value; } } - public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } - public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); - public override Quaternion APIDTarget { set { return; } } + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); - public override bool APIDActive { set { return; } } + // update to new position + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); - public override float APIDStrength { set { return; } } + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref primmass, + thispos.X, + thispos.Y, + thispos.Z); - public override float APIDDamping { set { return; } } + d.MassAdd(ref objdmass, ref primmass); - public override int VehicleType - { - // we may need to put a fake on this - get - { - if (m_vehicle == null) - return (int)Vehicle.TYPE_NONE; - else - return (int)m_vehicle.Type; - } - set + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) { - AddChange(changes.VehicleType, value); + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); } - } - - public override void VehicleFloatParam(int param, float value) - { - strVehicleFloatParam fp = new strVehicleFloatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleFloatParam, fp); - } - - public override void VehicleVectorParam(int param, Vector3 value) - { - strVehicleVectorParam fp = new strVehicleVectorParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleVectorParam, fp); - } - - public override void VehicleRotationParam(int param, Quaternion value) - { - strVehicleQuatParam fp = new strVehicleQuatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleRotationParam, fp); - } - - public override void VehicleFlags(int param, bool value) - { - strVehicleBoolParam bp = new strVehicleBoolParam(); - bp.param = param; - bp.value = value; - AddChange(changes.VehicleFlags, bp); - } - public override void SetVehicle(object vdata) - { - AddChange(changes.SetVehicle, vdata); - } - public void SetAcceleration(Vector3 accel) - { - _acceleration = accel; - } + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); - public override void AddForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); - } - //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); - } + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); - public override void AddAngularForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); - } + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; } - public override void CrossingFailure() + private void FixInertia(Quaternion newrot) { - if (m_outbounds) - { - _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); - _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); - _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); - m_lastposition = _position; - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - m_lastVelocity = _velocity; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - - if(Body != IntPtr.Zero) - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - if (prim_geom != IntPtr.Zero) - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - - m_outbounds = false; - changeDisable(false); - base.RequestPhysicsterseUpdate(); - } - } - - public override void SetMomentum(Vector3 momentum) - { - } - - public override void SetMaterial(int pMaterial) - { - m_material = pMaterial; - mu = _parent_scene.m_materialContactsData[pMaterial].mu; - bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; - } - - public void setPrimForRemoval() - { - AddChange(changes.Remove, null); - } - - public override void link(PhysicsActor obj) - { - AddChange(changes.Link, obj); - } - - public override void delink() - { - AddChange(changes.DeLink, null); - } - - public override void LockAngularMotion(Vector3 axis) - { - // reverse the zero/non zero values for ODE. - if (axis.IsFinite()) - { - axis.X = (axis.X > 0) ? 1f : 0f; - axis.Y = (axis.Y > 0) ? 1f : 0f; - axis.Z = (axis.Z > 0) ? 1f : 0f; - m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - AddChange(changes.AngLock, axis); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); - } - } - - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - _parent_scene.AddCollisionEventReporting(this); - } - - public override void UnSubscribeEvents() - { - _parent_scene.RemoveCollisionEventReporting(this); - m_eventsubscription = 0; - } - - public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) - { - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); - - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - } - - public void SendCollisions() - { - if (CollisionEventsThisFrame == null) - return; - - base.SendCollisionUpdate(CollisionEventsThisFrame); - - if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) - CollisionEventsThisFrame = null; - else - CollisionEventsThisFrame = new CollisionEventUpdate(); - } - - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } - - - public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) - { - Name = primName; - LocalID = plocalID; - - m_vehicle = null; - - if (!pos.IsFinite()) - { - pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), - parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); - } - _position = pos; - givefakepos = 0; - - PID_D = parent_scene.bodyPIDD; - PID_G = parent_scene.bodyPIDG; - m_density = parent_scene.geomDefaultDensity; - // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; - body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - - prim_geom = IntPtr.Zero; - collide_geom = IntPtr.Zero; - Body = IntPtr.Zero; - - if (!size.IsFinite()) - { - size = new Vector3(0.5f, 0.5f, 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); - } - - if (size.X <= 0) size.X = 0.01f; - if (size.Y <= 0) size.Y = 0.01f; - if (size.Z <= 0) size.Z = 0.01f; - - _size = size; - - if (!QuaternionIsFinite(rotation)) - { - rotation = Quaternion.Identity; - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); - } - - _orientation = rotation; - givefakeori = 0; - - _pbs = pbs; - - _parent_scene = parent_scene; - m_targetSpace = IntPtr.Zero; - - if (pos.Z < 0) - { - m_isphysical = false; - } - else - { - m_isphysical = pisPhysical; - } - m_fakeisphysical = m_isphysical; - - m_isVolumeDetect = false; - - m_force = Vector3.Zero; - - m_iscolliding = false; - m_colliderfilter = 0; - m_softcolide = true; - m_NoColide = false; - - hasOOBoffsetFromMesh = false; - _triMeshData = IntPtr.Zero; - - m_shapetype = _shapeType; - - m_lastdoneSelected = false; - m_isSelected = false; - m_delaySelect = false; - - m_isphantom = pisPhantom; - m_fakeisphantom = pisPhantom; - - mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; - bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; - - CalcPrimBodyData(); - - m_building = true; // control must set this to false when done - - AddChange(changes.Add, null); - } - - private void resetCollisionAccounting() - { - m_collisionscore = 0; - } - - private void createAMotor(Vector3 axis) - { - if (Body == IntPtr.Zero) - return; - - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - - int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); - - if (axisnum <= 0) - return; - - // stop it - d.BodySetTorque(Body, 0, 0, 0); - d.BodySetAngularVel(Body, 0, 0, 0); - - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - - d.JointSetAMotorMode(Amotor, 0); - - d.JointSetAMotorNumAxes(Amotor, axisnum); - - // get current orientation to lock - - d.Quaternion dcur = d.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; - Vector3 ax; - - int i = 0; - int j = 0; - if (axis.X == 0) - { - ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X - // ODE should do this with axis relative to body 1 but seems to fail - d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, 0, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); - i++; - j = 256; // move to next axis set - } - - if (axis.Y == 0) - { - ax = (new Vector3(0, 1, 0)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - i++; - j += 256; - } - - if (axis.Z == 0) - { - ax = (new Vector3(0, 0, 1)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - } - } - - private bool setMesh(OdeScene parent_scene) - { - if (Body != IntPtr.Zero) - { - if (childPrim) - { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this, false); - } - } - else - { - DestroyBody(); - } - } - - bool convex; - if (m_shapetype == 0) - convex = false; - else - convex = true; - - IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex); - if (mesh == null) - { - m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); - return false; - } - - IntPtr vertices, indices; - int vertexCount, indexCount; - int vertexStride, triStride; - - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage - - if (vertexCount == 0 || indexCount == 0) - { - m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", - Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); - mesh.releaseSourceMeshData(); - return false; - } - - primOOBoffset = mesh.GetCentroid(); - hasOOBoffsetFromMesh = true; - - mesh.releaseSourceMeshData(); - - IntPtr geo = IntPtr.Zero; - - try - { - _triMeshData = d.GeomTriMeshDataCreate(); - - d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); - d.GeomTriMeshDataPreprocess(_triMeshData); - - _parent_scene.waitForSpaceUnlock(m_targetSpace); - geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); - } - - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - return false; - } - - SetGeom(geo); - return true; - } - - private void SetGeom(IntPtr geom) - { - prim_geom = geom; - //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - { - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCollideBits(prim_geom, 0); - d.GeomDisable(prim_geom); - } - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - - CalcPrimBodyData(); - - _parent_scene.geom_name_map[prim_geom] = Name; - _parent_scene.actor_name_map[prim_geom] = this; - - } - else - m_log.Warn("Setting bad Geom"); - } - - - /// - /// Create a geometry for the given mesh in the given target space. - /// - /// - /// If null, then a mesh is used that is based on the profile shape data. - private void CreateGeom() - { - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - - bool haveMesh = false; - hasOOBoffsetFromMesh = false; - m_NoColide = false; - - if (_parent_scene.needsMeshing(_pbs)) - { - haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims - if (!haveMesh) - m_NoColide = true; - } - - if (!haveMesh) - { - if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 - && _size.X == _size.Y && _size.Y == _size.Z) - { // it's a sphere - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); - return; - } - } - else - {// do it as a box - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - //Console.WriteLine(" CreateGeom 4"); - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); - } - catch (Exception e) - { - m_log.Warn("[PHYSICS]: Create box failed: {0}", e); - return; - } - } - } - } - - /// - /// Set a new geometry for this prim. - /// - /// - private void RemoveGeom() - { - if (prim_geom != IntPtr.Zero) - { - _parent_scene.geom_name_map.Remove(prim_geom); - _parent_scene.actor_name_map.Remove(prim_geom); - try - { - d.GeomDestroy(prim_geom); - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - } - // catch (System.AccessViolationException) - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); - } - - prim_geom = IntPtr.Zero; - } - else - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); - } - Body = IntPtr.Zero; - hasOOBoffsetFromMesh = false; - CalcPrimBodyData(); - } - - private void ChildSetGeom(OdePrim odePrim) - { - // well.. - DestroyBody(); - MakeBody(); - } - - //sets non physical prim m_targetSpace to right space in spaces grid for static prims - // should only be called for non physical prims unless they are becoming non physical - private void SetInStaticSpace(OdePrim prim) - { - IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); - prim.m_targetSpace = targetSpace; - d.GeomEnable(prim_geom); - } - - public void enableBodySoft() - { - if (!childPrim && !m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - m_collisionFlags = CollisionCategories.Land; - } - else - { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prm.prim_geom); - } - } - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prim_geom); - } - d.BodyEnable(Body); - } - } - m_disabled = false; - resetCollisionAccounting(); // this sets m_disable to false - } - - private void disableBodySoft() - { - m_disabled = true; - if (!childPrim) - { - if (m_isphysical && Body != IntPtr.Zero) - { - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prm.prim_geom); - } - } - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prim_geom); - } - - d.BodyDisable(Body); - } - } - } - - private void MakeBody() - { - if (!m_isphysical) // only physical get bodies - return; - - if (childPrim) // child prims don't get bodies; - return; - - if (m_building) - return; - - if (prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); - return; - } - - if (Body != IntPtr.Zero) - { - d.BodyDestroy(Body); - Body = IntPtr.Zero; - m_log.Warn("[PHYSICS]: MakeBody called having a body"); - } - - - if (d.GeomGetBody(prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); - } - - d.Matrix3 mymat = new d.Matrix3(); - d.Quaternion myrot = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; d.Mass objdmass = new d.Mass { }; - - Body = d.BodyCreate(_parent_scene.world); - - DMassDup(ref primdMass, out objdmass); - - // rotate inertia - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); - - // set the body rotation and position - d.BodySetRotation(Body, ref mymat); - - // recompute full object inertia if needed - if (childrenPrim.Count > 0) - { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - d.Mass tmpdmass = new d.Mass { }; - Vector3 rcm; - - rcm.X = _position.X + objdmass.c.X; - rcm.Y = _position.Y + objdmass.c.Y; - rcm.Z = _position.Z + objdmass.c.Z; - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); - continue; - } - - DMassCopy(ref prm.primdMass, ref tmpdmass); - - // apply prim current rotation to inertia - quat.X = prm._orientation.X; - quat.Y = prm._orientation.Y; - quat.Z = prm._orientation.Z; - quat.W = prm._orientation.W; - d.RfromQ(out mat, ref quat); - d.MassRotate(ref tmpdmass, ref mat); - - Vector3 ppos = prm._position; - ppos.X += tmpdmass.c.X - rcm.X; - ppos.Y += tmpdmass.c.Y - rcm.Y; - ppos.Z += tmpdmass.c.Z - rcm.Z; - - // refer inertia to root prim center of mass position - d.MassTranslate(ref tmpdmass, - ppos.X, - ppos.Y, - ppos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia - // fix prim colision cats - - if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); - } - - d.GeomClearOffset(prm.prim_geom); - d.GeomSetBody(prm.prim_geom, Body); - prm.Body = Body; - d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation - } - } - } - - d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset - // associate root geom with body - d.GeomSetBody(prim_geom, Body); - - d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); - d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); - + d.Vector3 dobjpos; + d.Vector3 thispos; + + d.BodyGetMass(Body, out objdmass); + + // get prim own inertia in its local frame + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + // transform to object frame + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // update to new orientation + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - myrot.W = -myrot.W; - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); d.BodySetMass(Body, ref objdmass); _mass = objdmass.mass; - - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode(Body, false); - - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - // d.BodySetLinearDampingThreshold(Body, 0.01f); - // d.BodySetAngularDampingThreshold(Body, 0.001f); - d.BodySetDamping(Body, .002f, .002f); - - - if (m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - if (d.SpaceQuery(m_targetSpace, prim_geom)) - d.SpaceRemove(m_targetSpace, prim_geom); - } - - - if (childrenPrim.Count == 0) - { - collide_geom = prim_geom; - m_targetSpace = _parent_scene.ActiveSpace; - d.SpaceAdd(m_targetSpace, prim_geom); - } - else - { - m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); - d.HashSpaceSetLevels(m_targetSpace, -2, 8); - d.SpaceSetSublevel(m_targetSpace, 3); - d.SpaceSetCleanup(m_targetSpace, false); - d.SpaceAdd(m_targetSpace, prim_geom); - collide_geom = m_targetSpace; - } - - if (m_delaySelect) - { - m_isSelected = true; - m_delaySelect = false; - } - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom == IntPtr.Zero) - continue; - - Vector3 ppos = prm._position; - d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position - - if (prm.m_targetSpace != m_targetSpace) - { - if (prm.m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); - if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) - d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); - } - prm.m_targetSpace = m_targetSpace; - d.SpaceAdd(m_targetSpace, prm.prim_geom); - } - - if (m_isSelected || m_disabled) - { - prm.m_collisionCategories &= ~CollisionCategories.Body; - prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); - d.GeomDisable(prm.prim_geom); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - prm.m_collisionCategories = 0; - prm.m_collisionFlags = CollisionCategories.Land; - } - else - { - prm.m_collisionCategories |= CollisionCategories.Body; - prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - d.GeomEnable(prm.prim_geom); - } - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - d.GeomEnable(prm.prim_geom); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); - } - prm.m_collisionscore = 0; - - if(!m_disabled) - prm.m_disabled = false; - - _parent_scene.addActivePrim(prm); - } - } - - // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) - { - createAMotor(m_angularlock); - } - - if (m_isSelected || m_disabled) - { - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); - - d.GeomDisable(prim_geom); - d.BodyDisable(Body); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - m_collisionFlags = CollisionCategories.Land; - } - else - { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - - d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - } - - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - - m_collisionscore = 0; - - m_softcolide = true; - _parent_scene.addActivePrim(this); - _parent_scene.addActiveGroups(this); - } - - private void DestroyBody() - { - if (Body != IntPtr.Zero) - { - _parent_scene.remActivePrim(this); - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - UpdateDataFromGeom(); - d.GeomSetBody(prim_geom, IntPtr.Zero); - SetInStaticSpace(this); - } - - if (!childPrim) - { - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - _parent_scene.remActivePrim(prm); - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - prm.UpdateDataFromGeom(); - SetInStaticSpace(prm); - } - prm.Body = IntPtr.Zero; - prm._mass = prm.primMass; - prm.m_collisionscore = 0; - } - } - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - _parent_scene.remActiveGroup(this); - d.BodyDestroy(Body); - } - Body = IntPtr.Zero; - } - _mass = primMass; - m_collisionscore = 0; - } - - #region Mass Calculation - - private float CalculatePrimVolume() - { - float volume = _size.X * _size.Y * _size.Z; // default - float tmp; - - float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - - switch (_pbs.ProfileShape) - { - case ProfileShape.Square: - // default box - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - - hollowVolume *= (0.5f * .5f); - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - //a tube - - volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume * tmp * tmp; - - if (hollowAmount > 0.0) - { - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - hollowVolume *= 0.5f * 0.5f; - break; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - break; - - case ProfileShape.Circle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.78539816339f; // elipse base - - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - case ProfileShape.HalfCircle: - if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.52359877559829887307710723054658f; - } - break; - - case ProfileShape.EquilateralTriangle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.32475953f; - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.32475953f; - volume *= 0.01f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - default: - break; - } - - float taperX1; - float taperY1; - float taperX; - float taperY; - float pathBegin; - float pathEnd; - float profileBegin; - float profileEnd; - - if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) - { - taperX1 = _pbs.PathScaleX * 0.01f; - if (taperX1 > 1.0f) - taperX1 = 2.0f - taperX1; - taperX = 1.0f - taperX1; - - taperY1 = _pbs.PathScaleY * 0.01f; - if (taperY1 > 1.0f) - taperY1 = 2.0f - taperY1; - taperY = 1.0f - taperY1; - } - else - { - taperX = _pbs.PathTaperX * 0.01f; - if (taperX < 0.0f) - taperX = -taperX; - taperX1 = 1.0f - taperX; - - taperY = _pbs.PathTaperY * 0.01f; - if (taperY < 0.0f) - taperY = -taperY; - taperY1 = 1.0f - taperY; - } - - volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); - - pathBegin = (float)_pbs.PathBegin * 2.0e-5f; - pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; - volume *= (pathEnd - pathBegin); - - // this is crude aproximation - profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; - profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; - volume *= (profileEnd - profileBegin); - - return volume; - } - - - private void CalcPrimBodyData() - { - float volume; - - if (prim_geom == IntPtr.Zero) - { - // Ubit let's have a initial basic OOB - primOOBsize.X = _size.X; - primOOBsize.Y = _size.Y; - primOOBsize.Z = _size.Z; - primOOBoffset = Vector3.Zero; - } - else - { - d.AABB AABB; - d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom - - primOOBsize.X = (AABB.MaxX - AABB.MinX); - primOOBsize.Y = (AABB.MaxY - AABB.MinY); - primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); - if (!hasOOBoffsetFromMesh) - { - primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; - primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; - primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; - } - } - - // also its own inertia and mass - // keep using basic shape mass for now - volume = CalculatePrimVolume(); - - primMass = m_density * volume; - - if (primMass <= 0) - primMass = 0.0001f;//ckrinke: Mass must be greater then zero. - if (primMass > _parent_scene.maximumMassObject) - primMass = _parent_scene.maximumMassObject; - - _mass = primMass; // just in case - - d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); - - d.MassTranslate(ref primdMass, - primOOBoffset.X, - primOOBoffset.Y, - primOOBoffset.Z); - - primOOBsize *= 0.5f; // let obb size be a corner coords - primOOBradiusSQ = primOOBsize.LengthSquared(); - } - - - #endregion - - - /// - /// Add a child prim to this parent prim. - /// - /// Child prim - // I'm the parent - // prim is the child - public void ParentPrim(OdePrim prim) - { - //Console.WriteLine("ParentPrim " + m_primName); - if (this.m_localID != prim.m_localID) - { - DestroyBody(); // for now we need to rebuil entire object on link change - - lock (childrenPrim) - { - // adopt the prim - if (!childrenPrim.Contains(prim)) - childrenPrim.Add(prim); - - // see if this prim has kids and adopt them also - // should not happen for now - foreach (OdePrim prm in prim.childrenPrim) - { - if (!childrenPrim.Contains(prm)) - { - if (prm.Body != IntPtr.Zero) - { - if (prm.prim_geom != IntPtr.Zero) - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - if (prm.Body != prim.Body) - prm.DestroyBody(); // don't loose bodies around - prm.Body = IntPtr.Zero; - } - - childrenPrim.Add(prm); - prm._parent = this; - } - } - } - //Remove old children from the prim - prim.childrenPrim.Clear(); - - if (prim.Body != IntPtr.Zero) - { - if (prim.prim_geom != IntPtr.Zero) - d.GeomSetBody(prim.prim_geom, IntPtr.Zero); - prim.DestroyBody(); // don't loose bodies around - prim.Body = IntPtr.Zero; - } - - prim.childPrim = true; - prim._parent = this; - - MakeBody(); // full nasty reconstruction - } - } - - private void UpdateChildsfromgeom() - { - if (childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.UpdateDataFromGeom(); - } - } - - private void UpdateDataFromGeom() - { - if (prim_geom != IntPtr.Zero) - { - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - d.Quaternion qtmp = new d.Quaternion { }; - d.GeomCopyQuaternion(prim_geom, out qtmp); - _orientation.W = qtmp.W; - _orientation.X = qtmp.X; - _orientation.Y = qtmp.Y; - _orientation.Z = qtmp.Z; - } - } - - private void ChildDelink(OdePrim odePrim, bool remakebodies) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) // delinking the root prim - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) - { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); - } - if (newroot != null) - { - newroot.childPrim = false; - newroot._parent = null; - if (remakebodies) - newroot.MakeBody(); - } - } - } - - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - // odePrim.UpdateDataFromGeom(); - if (remakebodies) - odePrim.MakeBody(); - } - } - if (remakebodies) - MakeBody(); - } - - protected void ChildRemove(OdePrim odePrim, bool reMakeBody) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) - { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); - } - if (newroot != null) - { - newroot.childPrim = false; - newroot._parent = null; - newroot.MakeBody(); - } - } - if (reMakeBody) - MakeBody(); - return; - } - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - if (reMakeBody) - odePrim.MakeBody(); - } - } - MakeBody(); - } - - #region changes - - private void changeadd() - { - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - - if (!m_isphysical) - SetInStaticSpace(this); - } - - if (m_isphysical && Body == IntPtr.Zero) - { - MakeBody(); - } - } - - private void changeAngularLock(Vector3 newLock) - { - // do we have a Physical object? - if (Body != IntPtr.Zero) - { - //Check that we have a Parent - //If we have a parent then we're not authorative here - if (_parent == null) - { - if (!newLock.ApproxEquals(Vector3.One, 0f)) - { - createAMotor(newLock); - } - else - { - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - } - } - } - // Store this for later in case we get turned into a separate body - m_angularlock = newLock; - } - - private void changeLink(OdePrim NewParent) - { - if (_parent == null && NewParent != null) - { - NewParent.ParentPrim(this); - } - else if (_parent != null) - { - if (_parent is OdePrim) - { - if (NewParent != _parent) - { - (_parent as OdePrim).ChildDelink(this, false); // for now... - childPrim = false; - - if (NewParent != null) - { - NewParent.ParentPrim(this); - } - } - } - } - _parent = NewParent; - } - - - private void Stop() - { - if (!childPrim) - { - m_force = Vector3.Zero; - m_forceacc = Vector3.Zero; - m_angularForceacc = Vector3.Zero; - _torque = Vector3.Zero; - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - _target_velocity = Vector3.Zero; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - } - - if (Body != IntPtr.Zero) - { - d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetTorque(Body, 0f, 0f, 0f); - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetAngularVel(Body, 0f, 0f, 0f); - } - } - - - private void changePhantomStatus(bool newval) - { - m_isphantom = newval; - - if (m_isSelected) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - if (m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; // should never happen - } - - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; - } - } - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - if(!m_isSelected) - d.GeomEnable(prm.prim_geom); - } - } - } - - if (!m_disabled && prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - if(!m_isSelected) - d.GeomEnable(prim_geom); - } - } - - private void changeSelectedStatus(bool newval) - { - if (m_lastdoneSelected == newval) - return; - - m_lastdoneSelected = newval; - DoSelectedStatus(newval); - } - - private void CheckDelaySelect() - { - if (m_delaySelect) - { - DoSelectedStatus(m_isSelected); - } - } - - private void DoSelectedStatus(bool newval) - { - m_isSelected = newval; - Stop(); - - if (newval) - { - if (!childPrim && Body != IntPtr.Zero) - d.BodyDisable(Body); - - if (m_delaySelect || m_isphysical) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != null) - { - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prm.prim_geom); - } - prm.m_delaySelect = false; - } - } - - if (prim_geom != null) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prim_geom); - } - - m_delaySelect = false; - } - else if(!m_isphysical) - { - m_delaySelect = true; - } - } - else - { - if (!childPrim && Body != IntPtr.Zero && !m_disabled) - d.BodyEnable(Body); - - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - if(m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; - } - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; - } - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prm.prim_geom); - } - prm.m_delaySelect = false; - prm.m_softcolide = true; - } - } - - if (!m_disabled && prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prim_geom); - } - - m_delaySelect = false; - m_softcolide = true; - } - - resetCollisionAccounting(); - } - - private void changePosition(Vector3 newPos) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _position = newPos; - } - } - else - { - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (prim_geom != IntPtr.Zero) - { - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } - } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; - // changeSelectedStatus(); - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changeOrientation(Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _orientation = newOri; - } - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } - } - } - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim && m_building) // inertia is messed, must rebuild - { - _position = newPos; - _orientation = newOri; - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); - // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } - - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } - } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - - m_softcolide = true; - resetCollisionAccounting(); - } - - - private void changeDisable(bool disable) - { - if (disable) - { - if (!m_disabled) - disableBodySoft(); - } - else - { - if (m_disabled) - enableBodySoft(); - } - } - - private void changePhysicsStatus(bool NewStatus) - { - CheckDelaySelect(); - - m_isphysical = NewStatus; - - if (!childPrim) - { - if (NewStatus) - { - if (Body == IntPtr.Zero) - MakeBody(); - } - else - { - if (Body != IntPtr.Zero) - { - DestroyBody(); - } - Stop(); - } - } - - resetCollisionAccounting(); - } - - private void changeprimsizeshape() - { - CheckDelaySelect(); - - OdePrim parent = (OdePrim)_parent; - - bool chp = childPrim; - - if (chp) - { - if (parent != null) - { - parent.DestroyBody(); - } - } - else - { - DestroyBody(); - } - - RemoveGeom(); - - // we don't need to do space calculation because the client sends a position update also. - if (_size.X <= 0) - _size.X = 0.01f; - if (_size.Y <= 0) - _size.Y = 0.01f; - if (_size.Z <= 0) - _size.Z = 0.01f; - // Construction of new prim - - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - } - - if (chp) - { - if (parent != null) - { - parent.MakeBody(); - } - } - else - MakeBody(); - - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changeSize(Vector3 newSize) - { - _size = newSize; - changeprimsizeshape(); - } - - private void changeShape(PrimitiveBaseShape newShape) - { - if(newShape != null) - _pbs = newShape; - changeprimsizeshape(); - } - - private void changeFloatOnWater(bool newval) - { - m_collidesWater = newval; - - if (prim_geom != IntPtr.Zero && !m_isphantom) - { - if (m_collidesWater) - { - m_collisionFlags |= CollisionCategories.Water; - } - else - { - m_collisionFlags &= ~CollisionCategories.Water; - } - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - } - - private void changeSetTorque(Vector3 newtorque) - { - if (!m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - } - _torque = newtorque; - } - } - - private void changeForce(Vector3 force) - { - m_force = force; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - - private void changeAddForce(Vector3 force) - { - m_forceacc += force; - if (!m_isSelected) - { - lock (this) - { - //m_log.Info("[PHYSICS]: dequeing forcelist"); - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - - m_collisionscore = 0; - } - } - - private void changeAddAngularForce(Vector3 aforce) - { - m_angularForceacc += aforce; - if (!m_isSelected) - { - lock (this) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - m_collisionscore = 0; - } - } - - private void changevelocity(Vector3 newVel) - { - if (!m_isSelected) - { - if (Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); - } - //resetCollisionAccounting(); - } - _velocity = newVel; - } - - private void changeVolumedetetion(bool newVolDtc) - { - m_isVolumeDetect = newVolDtc; - } - - protected void changeBuilding(bool newbuilding) - { - if ((bool)newbuilding) - { - m_building = true; - if (!childPrim) - DestroyBody(); - } - else - { - m_building = false; - CheckDelaySelect(); - if (!childPrim) - MakeBody(); - } - if (!childPrim && childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.changeBuilding(m_building); // call directly - } - } - - public void changeSetVehicle(VehicleData vdata) - { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - m_vehicle.DoSetVehicle(vdata); - } - private void changeVehicleType(int value) - { - if (value == (int)Vehicle.TYPE_NONE) - { - if (m_vehicle != null) - m_vehicle = null; - } - else - { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - - m_vehicle.ProcessTypeChange((Vehicle)value); - } - } - - private void changeVehicleFloatParam(strVehicleFloatParam fp) - { - if (m_vehicle == null) - return; - - m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); - } - - private void changeVehicleVectorParam(strVehicleVectorParam vp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); - } - - private void changeVehicleRotationParam(strVehicleQuatParam qp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); - } - - private void changeVehicleFlags(strVehicleBoolParam bp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVehicleFlags(bp.param, bp.value); - } - - #endregion - - public void Move() - { - if (!childPrim && m_isphysical && Body != IntPtr.Zero && - !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) - // !m_disabled && !m_isSelected && !m_building && !m_outbounds) - { -// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 - - float timestep = _parent_scene.ODE_STEPSIZE; - - // check outside region - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - - if (lpos.Z < -100 || lpos.Z > 100000f) - { - m_outbounds = true; - - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - return; - } - - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if(lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if(lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } - - if(m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; - - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - base.RequestPhysicsterseUpdate(); - return; - } - - - float fx = 0; - float fy = 0; - float fz = 0; - - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(); - } - else - { - float m_mass = _mass; - - // fz = 0f; - //m_log.Info(m_collisionFlags.ToString()); - if (m_usePID) - { - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } // end if (m_usePID) - - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID) - { - //Console.WriteLine("Hover " + Name); - - // If we're using the PID controller, then we have no gravity - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; - - } // end switch (m_PIDHoverType) - - - _target_velocity = - new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - // ? d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } - else - { - float b = (1.0f - m_buoyancy); - fx = _parent_scene.gravityx * b; - fy = _parent_scene.gravityy * b; - fz = _parent_scene.gravityz * b; - } - - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; - - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; - - m_forceacc = Vector3.Zero; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - d.BodyAddForce(Body, fx, fy, fz); - //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - - Vector3 trq; - - trq = _torque; - trq += m_angularForceacc; - m_angularForceacc = Vector3.Zero; - if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) - { - d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); - } - - } - } - else - { // is not physical, or is not a body or is selected - // _zeroPosition = d.BodyGetPosition(Body); - return; - //Console.WriteLine("Nothing " + Name); - - } - } - - - public void UpdatePositionAndVelocity(float simulatedtime) - { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null && !m_disabled && !m_building && !m_outbounds) - { - if (Body != IntPtr.Zero) - { - Vector3 pv = Vector3.Zero; - bool lastZeroFlag = _zeroFlag; - - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - - - d.Quaternion ori; - d.GeomCopyQuaternion(prim_geom, out ori); - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 rotvel = d.BodyGetAngularVel(Body); - - if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) - && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) - && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) - && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) - && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) - && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) - ) - { - _zeroFlag = true; - //Console.WriteLine("ZFT 2"); - m_throttleUpdates = false; - } - else - { - //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); - _zeroFlag = false; - m_lastUpdateSent = false; - //m_throttleUpdates = false; - } - - if (_zeroFlag) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - m_rotationalVelocity = pv; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; - } - } - else - { - if (lastZeroFlag != _zeroFlag) - { - base.RequestPhysicsterseUpdate(); - } - - m_lastVelocity = _velocity; - - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - - _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - { - m_rotationalVelocity = pv; - } - else - { - m_rotationalVelocity.X = rotvel.X; - m_rotationalVelocity.Y = rotvel.Y; - m_rotationalVelocity.Z = rotvel.Z; - } - - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) - { - m_lastposition = _position; - m_lastorientation = _orientation; - base.RequestPhysicsterseUpdate(); - } - else - { - throttleCounter++; - } - } - } - else if (!m_lastUpdateSent || !_zeroFlag) - { - // Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; - - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; - } - } - } - } - - internal static bool QuaternionIsFinite(Quaternion q) - { - if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) - return false; - if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) - return false; - if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) - return false; - if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) - return false; - return true; - } - - internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) - { - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } - - private static void DMassDup(ref d.Mass src, out d.Mass dst) - { - dst = new d.Mass { }; - - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } - private void donullchange() - { - } - - public bool DoAChange(changes what, object arg) - { - if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) - { - return false; - } - - // nasty switch - switch (what) - { - case changes.Add: - changeadd(); - break; - case changes.Remove: - //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... - //When we return true, it destroys all of the prims in the linkset anyway - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildRemove(this, false); - } - else - ChildRemove(this, false); - - m_vehicle = null; - RemoveGeom(); - m_targetSpace = IntPtr.Zero; - if (m_eventsubscription > 0) - UnSubscribeEvents(); - return true; - - case changes.Link: - OdePrim tmp = (OdePrim)arg; - changeLink(tmp); - break; - - case changes.DeLink: - changeLink(null); - break; - - case changes.Position: - changePosition((Vector3)arg); - break; - - case changes.Orientation: - changeOrientation((Quaternion)arg); - break; - - case changes.PosOffset: - donullchange(); - break; - - case changes.OriOffset: - donullchange(); - break; - - case changes.Velocity: - changevelocity((Vector3)arg); - break; - - // case changes.Acceleration: - // changeacceleration((Vector3)arg); - // break; - // case changes.AngVelocity: - // changeangvelocity((Vector3)arg); - // break; - - case changes.Force: - changeForce((Vector3)arg); - break; - - case changes.Torque: - changeSetTorque((Vector3)arg); - break; - - case changes.AddForce: - changeAddForce((Vector3)arg); - break; - - case changes.AddAngForce: - changeAddAngularForce((Vector3)arg); - break; - - case changes.AngLock: - changeAngularLock((Vector3)arg); - break; - - case changes.Size: - changeSize((Vector3)arg); - break; - - case changes.Shape: - changeShape((PrimitiveBaseShape)arg); - break; - - case changes.CollidesWater: - changeFloatOnWater((bool)arg); - break; - - case changes.VolumeDtc: - changeVolumedetetion((bool)arg); - break; - - case changes.Phantom: - changePhantomStatus((bool)arg); - break; - - case changes.Physical: - changePhysicsStatus((bool)arg); - break; - - case changes.Selected: - changeSelectedStatus((bool)arg); - break; - - case changes.disabled: - changeDisable((bool)arg); - break; - - case changes.building: - changeBuilding((bool)arg); - break; - - case changes.VehicleType: - changeVehicleType((int)arg); - break; - - case changes.VehicleFlags: - changeVehicleFlags((strVehicleBoolParam) arg); - break; - - case changes.VehicleFloatParam: - changeVehicleFloatParam((strVehicleFloatParam) arg); - break; - - case changes.VehicleVectorParam: - changeVehicleVectorParam((strVehicleVectorParam) arg); - break; - - case changes.VehicleRotationParam: - changeVehicleRotationParam((strVehicleQuatParam) arg); - break; - - case changes.SetVehicle: - changeSetVehicle((VehicleData) arg); - break; - case changes.Null: - donullchange(); - break; - - default: - donullchange(); - break; - } - return false; - } - - public void AddChange(changes what, object arg) - { - _parent_scene.AddChange((PhysicsActor) this, what, arg); - } - - - private struct strVehicleBoolParam - { - public int param; - public bool value; - } - - private struct strVehicleFloatParam - { - public int param; - public float value; - } - - private struct strVehicleQuatParam - { - public int param; - public Quaternion value; - } - - private struct strVehicleVectorParam - { - public int param; - public Vector3 value; } - } + + + #region Mass Calculation + + private float CalculatePrimVolume() + { + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.52359877559829887307710723054658f; + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + return volume; + } + + + private void CalcPrimBodyData() + { + float volume; + + if (prim_geom == IntPtr.Zero) + { + // Ubit let's have a initial basic OOB + primOOBsize.X = _size.X; + primOOBsize.Y = _size.Y; + primOOBsize.Z = _size.Z; + primOOBoffset = Vector3.Zero; + } + else + { + d.AABB AABB; + d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom + + primOOBsize.X = (AABB.MaxX - AABB.MinX); + primOOBsize.Y = (AABB.MaxY - AABB.MinY); + primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); + if (!hasOOBoffsetFromMesh) + { + primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; + primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; + primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; + } + } + + // also its own inertia and mass + // keep using basic shape mass for now + volume = CalculatePrimVolume(); + + primMass = m_density * volume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + _mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); + + d.MassTranslate(ref primdMass, + primOOBoffset.X, + primOOBoffset.Y, + primOOBoffset.Z); + + primOOBsize *= 0.5f; // let obb size be a corner coords + primOOBradiusSQ = primOOBsize.LengthSquared(); + } + + + #endregion + + + /// + /// Add a child prim to this parent prim. + /// + /// Child prim + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) + { + //Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) + { + DestroyBody(); // for now we need to rebuil entire object on link change + + lock (childrenPrim) + { + // adopt the prim + if (!childrenPrim.Contains(prim)) + childrenPrim.Add(prim); + + // see if this prim has kids and adopt them also + // should not happen for now + foreach (OdePrim prm in prim.childrenPrim) + { + if (!childrenPrim.Contains(prm)) + { + if (prm.Body != IntPtr.Zero) + { + if (prm.prim_geom != IntPtr.Zero) + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + if (prm.Body != prim.Body) + prm.DestroyBody(); // don't loose bodies around + prm.Body = IntPtr.Zero; + } + + childrenPrim.Add(prm); + prm._parent = this; + } + } + } + //Remove old children from the prim + prim.childrenPrim.Clear(); + + if (prim.Body != IntPtr.Zero) + { + if (prim.prim_geom != IntPtr.Zero) + d.GeomSetBody(prim.prim_geom, IntPtr.Zero); + prim.DestroyBody(); // don't loose bodies around + prim.Body = IntPtr.Zero; + } + + prim.childPrim = true; + prim._parent = this; + + MakeBody(); // full nasty reconstruction + } + } + + private void UpdateChildsfromgeom() + { + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.UpdateDataFromGeom(); + } + } + + private void UpdateDataFromGeom() + { + if (prim_geom != IntPtr.Zero) + { + d.Quaternion qtmp = new d.Quaternion { }; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.W = qtmp.W; + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + } + } + + private void ChildDelink(OdePrim odePrim, bool remakebodies) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) // delinking the root prim + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + if (remakebodies) + newroot.MakeBody(); + } + } + } + + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + // odePrim.UpdateDataFromGeom(); + if (remakebodies) + odePrim.MakeBody(); + } + } + if (remakebodies) + MakeBody(); + } + + protected void ChildRemove(OdePrim odePrim, bool reMakeBody) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + newroot.MakeBody(); + } + } + if (reMakeBody) + MakeBody(); + return; + } + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + if (reMakeBody) + odePrim.MakeBody(); + } + } + MakeBody(); + } + + #region changes + + private void changeadd() + { + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + + if (!m_isphysical) + SetInStaticSpace(this); + } + + if (m_isphysical && Body == IntPtr.Zero) + { + MakeBody(); + } + } + + private void changeAngularLock(Vector3 newLock) + { + // do we have a Physical object? + if (Body != IntPtr.Zero) + { + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) + { + if (!newLock.ApproxEquals(Vector3.One, 0f)) + { + createAMotor(newLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = newLock; + } + + private void changeLink(OdePrim NewParent) + { + if (_parent == null && NewParent != null) + { + NewParent.ParentPrim(this); + } + else if (_parent != null) + { + if (_parent is OdePrim) + { + if (NewParent != _parent) + { + (_parent as OdePrim).ChildDelink(this, false); // for now... + childPrim = false; + + if (NewParent != null) + { + NewParent.ParentPrim(this); + } + } + } + } + _parent = NewParent; + } + + + private void Stop() + { + if (!childPrim) + { + m_force = Vector3.Zero; + m_forceacc = Vector3.Zero; + m_angularForceacc = Vector3.Zero; + _torque = Vector3.Zero; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + } + + if (Body != IntPtr.Zero) + { + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetTorque(Body, 0f, 0f, 0f); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetAngularVel(Body, 0f, 0f, 0f); + } + } + + + private void changePhantomStatus(bool newval) + { + m_isphantom = newval; + + if (m_isSelected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; // should never happen + } + + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } + } + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + if(!m_isSelected) + d.GeomEnable(prm.prim_geom); + } + } + } + + if (!m_disabled && prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + if(!m_isSelected) + d.GeomEnable(prim_geom); + } + } + + private void changeSelectedStatus(bool newval) + { + if (m_lastdoneSelected == newval) + return; + + m_lastdoneSelected = newval; + DoSelectedStatus(newval); + } + + private void CheckDelaySelect() + { + if (m_delaySelect) + { + DoSelectedStatus(m_isSelected); + } + } + + private void DoSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); + + if (newval) + { + if (!childPrim && Body != IntPtr.Zero) + d.BodyDisable(Body); + + if (m_delaySelect || m_isphysical) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != null) + { + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prm.prim_geom); + } + prm.m_delaySelect = false; + } + } + + if (prim_geom != null) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prim_geom); + } + + m_delaySelect = false; + } + else if(!m_isphysical) + { + m_delaySelect = true; + } + } + else + { + if (!childPrim && Body != IntPtr.Zero && !m_disabled) + d.BodyEnable(Body); + + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if(m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prm.prim_geom); + } + prm.m_delaySelect = false; + prm.m_softcolide = true; + } + } + + if (!m_disabled && prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prim_geom); + } + + m_delaySelect = false; + m_softcolide = true; + } + + resetCollisionAccounting(); + } + + private void changePosition(Vector3 newPos) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _position = newPos; + } + + else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) + { + FixInertia(newPos); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + // changeSelectedStatus(); + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changeOrientation(Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _orientation = newOri; + } + /* + else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) + { + FixInertia(_position, newOri); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + */ + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + } + } + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim && m_building) // inertia is messed, must rebuild + { + _position = newPos; + _orientation = newOri; + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + + m_softcolide = true; + resetCollisionAccounting(); + } + + + private void changeDisable(bool disable) + { + if (disable) + { + if (!m_disabled) + disableBodySoft(); + } + else + { + if (m_disabled) + enableBodySoft(); + } + } + + private void changePhysicsStatus(bool NewStatus) + { + CheckDelaySelect(); + + m_isphysical = NewStatus; + + if (!childPrim) + { + if (NewStatus) + { + if (Body == IntPtr.Zero) + MakeBody(); + } + else + { + if (Body != IntPtr.Zero) + { + DestroyBody(); + } + Stop(); + } + } + + resetCollisionAccounting(); + } + + private void changeprimsizeshape() + { + CheckDelaySelect(); + + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + // we don't need to do space calculation because the client sends a position update also. + if (_size.X <= 0) + _size.X = 0.01f; + if (_size.Y <= 0) + _size.Y = 0.01f; + if (_size.Z <= 0) + _size.Z = 0.01f; + // Construction of new prim + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changeSize(Vector3 newSize) + { + _size = newSize; + changeprimsizeshape(); + } + + private void changeShape(PrimitiveBaseShape newShape) + { + if(newShape != null) + _pbs = newShape; + changeprimsizeshape(); + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; + + if (prim_geom != IntPtr.Zero && !m_isphantom) + { + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } + else + { + m_collisionFlags &= ~CollisionCategories.Water; + } + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } + + private void changeSetTorque(Vector3 newtorque) + { + if (!m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + } + _torque = newtorque; + } + } + + private void changeForce(Vector3 force) + { + m_force = force; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeAddForce(Vector3 force) + { + m_forceacc += force; + if (!m_isSelected) + { + lock (this) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + + m_collisionscore = 0; + } + } + + private void changeAddAngularForce(Vector3 aforce) + { + m_angularForceacc += aforce; + if (!m_isSelected) + { + lock (this) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + m_collisionscore = 0; + } + } + + private void changevelocity(Vector3 newVel) + { + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; + } + + private void changeVolumedetetion(bool newVolDtc) + { + m_isVolumeDetect = newVolDtc; + } + + protected void changeBuilding(bool newbuilding) + { + if ((bool)newbuilding) + { + m_building = true; + if (!childPrim) + DestroyBody(); + } + else + { + m_building = false; + CheckDelaySelect(); + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } + } + + public void changeSetVehicle(VehicleData vdata) + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + m_vehicle.DoSetVehicle(vdata); + } + private void changeVehicleType(int value) + { + if (value == (int)Vehicle.TYPE_NONE) + { + if (m_vehicle != null) + m_vehicle = null; + } + else + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + + private void changeVehicleFloatParam(strVehicleFloatParam fp) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + } + + private void changeVehicleVectorParam(strVehicleVectorParam vp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + } + + private void changeVehicleRotationParam(strVehicleQuatParam qp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); + } + + private void changeVehicleFlags(strVehicleBoolParam bp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + } + + #endregion + + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) + // !m_disabled && !m_isSelected && !m_building && !m_outbounds) + { +// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + float timestep = _parent_scene.ODE_STEPSIZE; + + // check outside region + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; + + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if(m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } + + + float fx = 0; + float fy = 0; + float fz = 0; + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + } + else + { + float m_mass = _mass; + + // fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + if (m_usePID) + { + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID) + { + //Console.WriteLine("Hover " + Name); + + // If we're using the PID controller, then we have no gravity + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } + else + { + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } + + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; + + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; + + m_forceacc = Vector3.Zero; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + + Vector3 trq; + + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } + + } + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; + //Console.WriteLine("Nothing " + Name); + + } + } + + + public void UpdatePositionAndVelocity(float simulatedtime) + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null && !m_disabled && !m_building && !m_outbounds) + { + if (Body != IntPtr.Zero) + { + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + + if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) + && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) + && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) + && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) + && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) + && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) + ) + { + _zeroFlag = true; + //Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } + + if (_zeroFlag) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastVelocity = _velocity; + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + + _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + { + m_rotationalVelocity = pv; + } + else + { + m_rotationalVelocity.X = rotvel.X; + m_rotationalVelocity.Y = rotvel.Y; + m_rotationalVelocity.Z = rotvel.Z; + } + + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + m_lastposition = _position; + m_lastorientation = _orientation; + base.RequestPhysicsterseUpdate(); + } + else + { + throttleCounter++; + } + } + } + else if (!m_lastUpdateSent || !_zeroFlag) + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + } + } + + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } + + internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) + { + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + + internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) + { + // assumes object center of mass is zero + float smass = part.mass; + theobj.mass -= smass; + + smass *= 1.0f / (theobj.mass); ; + + theobj.c.X -= part.c.X * smass; + theobj.c.Y -= part.c.Y * smass; + theobj.c.Z -= part.c.Z * smass; + + theobj.I.M00 -= part.I.M00; + theobj.I.M01 -= part.I.M01; + theobj.I.M02 -= part.I.M02; + theobj.I.M10 -= part.I.M10; + theobj.I.M11 -= part.I.M11; + theobj.I.M12 -= part.I.M12; + theobj.I.M20 -= part.I.M20; + theobj.I.M21 -= part.I.M21; + theobj.I.M22 -= part.I.M22; + } + + private static void DMassDup(ref d.Mass src, out d.Mass dst) + { + dst = new d.Mass { }; + + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; + case changes.Remove: + //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... + //When we return true, it destroys all of the prims in the linkset anyway + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildRemove(this, false); + } + else + ChildRemove(this, false); + + m_vehicle = null; + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + if (m_eventsubscription > 0) + UnSubscribeEvents(); + return true; + + case changes.Link: + OdePrim tmp = (OdePrim)arg; + changeLink(tmp); + break; + + case changes.DeLink: + changeLink(null); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changevelocity((Vector3)arg); + break; + + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Phantom: + changePhantomStatus((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; + + case changes.VehicleType: + changeVehicleType((int)arg); + break; + + case changes.VehicleFlags: + changeVehicleFlags((strVehicleBoolParam) arg); + break; + + case changes.VehicleFloatParam: + changeVehicleFloatParam((strVehicleFloatParam) arg); + break; + + case changes.VehicleVectorParam: + changeVehicleVectorParam((strVehicleVectorParam) arg); + break; + + case changes.VehicleRotationParam: + changeVehicleRotationParam((strVehicleQuatParam) arg); + break; + + case changes.SetVehicle: + changeSetVehicle((VehicleData) arg); + break; + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor) this, what, arg); + } + + + private struct strVehicleBoolParam + { + public int param; + public bool value; + } + + private struct strVehicleFloatParam + { + public int param; + public float value; + } + + private struct strVehicleQuatParam + { + public int param; + public Quaternion value; + } + + private struct strVehicleVectorParam + { + public int param; + public Vector3 value; + } + } } \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 76d7746..3fc2de3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -169,7 +169,6 @@ namespace OpenSim.Region.Physics.OdePlugin float TerrainBounce = 0.1f; float TerrainFriction = 0.3f; - public float AvatarBounce = 0.3f; public float AvatarFriction = 0;// 0.9f * 0.5f; private const uint m_regionWidth = Constants.RegionSize; @@ -711,8 +710,8 @@ namespace OpenSim.Region.Physics.OdePlugin case (int)ActorTypes.Agent: p1.getContactData(ref contactdata1); p2.getContactData(ref contactdata2); - - bounce = contactdata1.bounce * contactdata2.bounce; + + bounce = 0; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); @@ -726,7 +725,8 @@ namespace OpenSim.Region.Physics.OdePlugin case (int)ActorTypes.Prim: p1.getContactData(ref contactdata1); p2.getContactData(ref contactdata2); - bounce = contactdata1.bounce * contactdata2.bounce; + + bounce = 0; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); @@ -751,7 +751,8 @@ namespace OpenSim.Region.Physics.OdePlugin case (int)ActorTypes.Agent: p1.getContactData(ref contactdata1); p2.getContactData(ref contactdata2); - bounce = contactdata1.bounce * contactdata2.bounce; + + bounce = 0; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); -- cgit v1.1 From 88d5cb6eef14bf9cd162f0ebfa71abc1b9337c05 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 24 Mar 2012 16:04:13 +0000 Subject: UbitOde let caller try to build meshs like done in chode. Changing this was a bad move i made. Variable colisions softness. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 7916 +++++++++++----------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 126 +- 2 files changed, 4058 insertions(+), 3984 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index bacd604..c4dc793 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1,3976 +1,4024 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -/* Revision 2011/12 by Ubit Umarov - * - * - */ - -/* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - */ - -//#define SPAM - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Threading; -using log4net; -using OpenMetaverse; -using OdeAPI; -using OpenSim.Framework; -using OpenSim.Region.Physics.Manager; - - -namespace OpenSim.Region.Physics.OdePlugin -{ - public class OdePrim : PhysicsActor - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - private bool m_isphysical; - private bool m_fakeisphysical; - private bool m_isphantom; +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* Revision 2011/12 by Ubit Umarov + * + * + */ + +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_isphysical; + private bool m_fakeisphysical; + private bool m_isphantom; private bool m_fakeisphantom; protected bool m_building; - protected bool m_forcePosOrRotation; - - private Quaternion m_lastorientation = new Quaternion(); - private Quaternion _orientation; - - private Vector3 _position; - private Vector3 _velocity; - private Vector3 _torque; - private Vector3 m_lastVelocity; - private Vector3 m_lastposition; - private Vector3 m_rotationalVelocity; - private Vector3 _size; - private Vector3 _acceleration; - private Vector3 m_angularlock = Vector3.One; - private IntPtr Amotor = IntPtr.Zero; - - private Vector3 m_force; - private Vector3 m_forceacc; - private Vector3 m_angularForceacc; - - private Vector3 m_PIDTarget; - private float m_PIDTau; - private float PID_D = 35f; - private float PID_G = 25f; - private bool m_usePID; - - // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), - // and are for non-VEHICLES only. - - private float m_PIDHoverHeight; - private float m_PIDHoverTau; - private bool m_useHoverPID; - private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; - private float m_targetHoverHeight; - private float m_groundHeight; - private float m_waterHeight; - private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - - private int body_autodisable_frames = 20; - - private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom - | CollisionCategories.Space - | CollisionCategories.Body - | CollisionCategories.Character - ); -// private bool m_collidesLand = true; - private bool m_collidesWater; - public bool m_returnCollisions; - private bool m_softcolide; - - private bool m_NoColide; // for now only for internal use for bad meshs - - // Default we're a Geometry - private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); - - // Default, Collide with Other Geometries, spaces and Bodies - private CollisionCategories m_collisionFlags = m_default_collisionFlags; - - public bool m_disabled; - - - public uint m_localID; - - private PrimitiveBaseShape _pbs; - public OdeScene _parent_scene; - - /// - /// The physics space which contains prim geometry - /// - public IntPtr m_targetSpace = IntPtr.Zero; - - public IntPtr prim_geom; - public IntPtr _triMeshData; - - private PhysicsActor _parent; - - private List childrenPrim = new List(); - - private bool m_iscolliding; - - public bool m_isSelected; - private bool m_delaySelect; - private bool m_lastdoneSelected; - public bool m_outbounds; - - internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - - private bool m_throttleUpdates; - private int throttleCounter; - public float m_collisionscore; - int m_colliderfilter = 0; - - public IntPtr collide_geom; // for objects: geom if single prim space it linkset - - private float m_density = 10.000006836f; // Aluminum g/cm3; - private byte m_shapetype; - public bool _zeroFlag; - private bool m_lastUpdateSent; - - public IntPtr Body = IntPtr.Zero; - public String Name { get; private set; } - private Vector3 _target_velocity; - - public Vector3 primOOBsize; // prim real dimensions from mesh - public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb - public float primOOBradiusSQ; - public d.Mass primdMass; // prim inertia information on it's own referencial - float primMass; // prim own mass - float _mass; // object mass acording to case - private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb - - public int givefakepos = 0; - private Vector3 fakepos; - public int givefakeori = 0; - private Quaternion fakeori; - - public int m_eventsubscription; - private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); - - public volatile bool childPrim; - - public ODEDynamics m_vehicle; - - internal int m_material = (int)Material.Wood; - private float mu; - private float bounce; - - /// - /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. - /// - public override bool IsPhysical // this is not reliable for internal use - { - get { return m_fakeisphysical; } - set - { - m_fakeisphysical = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - - if (!value) // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - AddChange(changes.Physical, value); - } - } - - public override bool Phantom // this is not reliable for internal use - { - get { return m_fakeisphantom; } - set - { - m_fakeisphantom = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - - AddChange(changes.Phantom, value); - } - } - - public override bool Building // this is not reliable for internal use - { - get { return m_building; } - set - { - if (value) - m_building = true; - AddChange(changes.building, value); - } - } - - public override void getContactData(ref ContactData cdata) - { - cdata.mu = mu; - cdata.bounce = bounce; - - // cdata.softcolide = m_softcolide; - cdata.softcolide = false; - - if (m_isphysical) - { - ODEDynamics veh; - if (_parent != null) - veh = ((OdePrim)_parent).m_vehicle; - else - veh = m_vehicle; - - if (veh != null && veh.Type != Vehicle.TYPE_NONE) - cdata.mu *= veh.FrictionFactor; - } - } - - public override int PhysicsActorType - { - get { return (int)ActorTypes.Prim; } - set { return; } - } - - public override bool SetAlwaysRun - { - get { return false; } - set { return; } - } - - public override uint LocalID - { - get - { - return m_localID; - } - set - { - //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); - m_localID = value; - } - } - - public override bool Grabbed - { - set { return; } - } - - public override bool Selected - { - set - { - if (value) - m_isSelected = value; // if true set imediatly to stop moves etc - AddChange(changes.Selected, value); - } - } - - public override bool Flying - { - // no flying prims for you - get { return false; } - set { } - } - - public override bool IsColliding - { - get { return m_iscolliding; } - set - { - if (value) - { - m_colliderfilter += 2; - if (m_colliderfilter > 2) - m_colliderfilter = 2; - } - else - { - m_colliderfilter--; - if (m_colliderfilter < 0) - m_colliderfilter = 0; - } - - if (m_colliderfilter == 0) - { - m_softcolide = false; - m_iscolliding = false; - } - else - m_iscolliding = true; - } - } - - public override bool CollidingGround - { - get { return false; } - set { return; } - } - - public override bool CollidingObj - { - get { return false; } - set { return; } - } - - public override bool ThrottleUpdates - { - get { return m_throttleUpdates; } - set { m_throttleUpdates = value; } - } - - public override bool Stopped - { - get { return _zeroFlag; } - } - - public override Vector3 Position - { - get - { - if (givefakepos > 0) - return fakepos; - else - return _position; - } - - set - { - fakepos = value; - givefakepos++; - AddChange(changes.Position, value); - } - } - - public override Vector3 Size - { - get { return _size; } - set - { - if (value.IsFinite()) - { - AddChange(changes.Size, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); - } - } - } - - public override float Mass - { - get { return _mass; } - } - - public override Vector3 Force - { - //get { return Vector3.Zero; } - get { return m_force; } - set - { - if (value.IsFinite()) - { - AddChange(changes.Force, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); - } - } - } - - public override void SetVolumeDetect(int param) - { - AddChange(changes.VolumeDtc, (param != 0)); - } - - public override Vector3 GeometricCenter - { - get - { - return Vector3.Zero; - } - } - - public override Vector3 CenterOfMass - { - get - { - d.Vector3 dtmp; - if (IsPhysical && !childPrim && Body != IntPtr.Zero) - { - dtmp = d.BodyGetPosition(Body); - return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - } - else if (prim_geom != IntPtr.Zero) - { - d.Quaternion dq; - d.GeomCopyQuaternion(prim_geom, out dq); - Quaternion q; - q.X = dq.X; - q.Y = dq.Y; - q.Z = dq.Z; - q.W = dq.W; - - Vector3 vtmp = primOOBoffset * q; - dtmp = d.GeomGetPosition(prim_geom); - return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); - } - else - return Vector3.Zero; - } - } - /* - public override Vector3 PrimOOBsize - { - get - { - return primOOBsize; - } - } - - public override Vector3 PrimOOBoffset - { - get - { - return primOOBoffset; - } - } - - public override float PrimOOBRadiusSQ - { - get - { - return primOOBradiusSQ; - } - } - */ - public override PrimitiveBaseShape Shape - { - set - { - AddChange(changes.Shape, value); - } - } - - public override byte PhysicsShapeType - { - get - { - return m_shapetype; - } - set - { - m_shapetype = value; - AddChange(changes.Shape, null); - } - } - - - public override Vector3 Velocity - { - get - { - if (_zeroFlag) - return Vector3.Zero; - return _velocity; - } - set - { - if (value.IsFinite()) - { - AddChange(changes.Velocity, value); - // _velocity = value; - - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); - } - - } - } - - public override Vector3 Torque - { - get - { - if (!IsPhysical || Body == IntPtr.Zero) - return Vector3.Zero; - - return _torque; - } - - set - { - if (value.IsFinite()) - { - AddChange(changes.Torque, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); - } - } - } - - public override float CollisionScore - { - get { return m_collisionscore; } - set { m_collisionscore = value; } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override Quaternion Orientation - { - get - { - if (givefakeori > 0) - return fakeori; - else - - return _orientation; - } - set - { - if (QuaternionIsFinite(value)) - { - fakeori = value; - givefakeori++; - AddChange(changes.Orientation, value); - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); - - } - } - - public override Vector3 Acceleration - { - get { return _acceleration; } - set { } - } - - public override Vector3 RotationalVelocity - { - get - { - Vector3 pv = Vector3.Zero; - if (_zeroFlag) - return pv; - m_lastUpdateSent = false; - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - return pv; - - return m_rotationalVelocity; - } - set - { - if (value.IsFinite()) - { - m_rotationalVelocity = value; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); - } - } - } - - - public override float Buoyancy - { - get { return m_buoyancy; } - set - { - m_buoyancy = value; - } - } - - public override bool FloatOnWater - { - set - { - AddChange(changes.CollidesWater, value); - } - } - - public override Vector3 PIDTarget - { - set - { - if (value.IsFinite()) - { - m_PIDTarget = value; - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); - } - } - - public override bool PIDActive { set { m_usePID = value; } } - public override float PIDTau { set { m_PIDTau = value; } } - - public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } - public override bool PIDHoverActive { set { m_useHoverPID = value; } } - public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } - public override float PIDHoverTau { set { m_PIDHoverTau = value; } } - - public override Quaternion APIDTarget { set { return; } } - - public override bool APIDActive { set { return; } } - - public override float APIDStrength { set { return; } } - - public override float APIDDamping { set { return; } } - - public override int VehicleType - { - // we may need to put a fake on this - get - { - if (m_vehicle == null) - return (int)Vehicle.TYPE_NONE; - else - return (int)m_vehicle.Type; - } - set - { - AddChange(changes.VehicleType, value); - } - } - - public override void VehicleFloatParam(int param, float value) - { - strVehicleFloatParam fp = new strVehicleFloatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleFloatParam, fp); - } - - public override void VehicleVectorParam(int param, Vector3 value) - { - strVehicleVectorParam fp = new strVehicleVectorParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleVectorParam, fp); - } - - public override void VehicleRotationParam(int param, Quaternion value) - { - strVehicleQuatParam fp = new strVehicleQuatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleRotationParam, fp); - } - - public override void VehicleFlags(int param, bool value) - { - strVehicleBoolParam bp = new strVehicleBoolParam(); - bp.param = param; - bp.value = value; - AddChange(changes.VehicleFlags, bp); - } - - public override void SetVehicle(object vdata) - { - AddChange(changes.SetVehicle, vdata); - } - public void SetAcceleration(Vector3 accel) - { - _acceleration = accel; - } - - public override void AddForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); - } - //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); - } - - public override void AddAngularForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); - } - } - - public override void CrossingFailure() - { - if (m_outbounds) - { - _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); - _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); - _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); - - m_lastposition = _position; - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - m_lastVelocity = _velocity; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - - if(Body != IntPtr.Zero) - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - if (prim_geom != IntPtr.Zero) - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - - m_outbounds = false; - changeDisable(false); - base.RequestPhysicsterseUpdate(); - } - } - - public override void SetMomentum(Vector3 momentum) - { - } - - public override void SetMaterial(int pMaterial) - { - m_material = pMaterial; - mu = _parent_scene.m_materialContactsData[pMaterial].mu; - bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; - } - - public void setPrimForRemoval() - { - AddChange(changes.Remove, null); - } - - public override void link(PhysicsActor obj) - { - AddChange(changes.Link, obj); - } - - public override void delink() - { - AddChange(changes.DeLink, null); - } - - public override void LockAngularMotion(Vector3 axis) - { - // reverse the zero/non zero values for ODE. - if (axis.IsFinite()) - { - axis.X = (axis.X > 0) ? 1f : 0f; - axis.Y = (axis.Y > 0) ? 1f : 0f; - axis.Z = (axis.Z > 0) ? 1f : 0f; - m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - AddChange(changes.AngLock, axis); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); - } - } - - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - _parent_scene.AddCollisionEventReporting(this); - } - - public override void UnSubscribeEvents() - { - _parent_scene.RemoveCollisionEventReporting(this); - m_eventsubscription = 0; - } - - public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) - { - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); - - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - } - - public void SendCollisions() - { - if (CollisionEventsThisFrame == null) - return; - - base.SendCollisionUpdate(CollisionEventsThisFrame); - - if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) - CollisionEventsThisFrame = null; - else - CollisionEventsThisFrame = new CollisionEventUpdate(); - } - - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } - - - public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) - { - Name = primName; - LocalID = plocalID; - - m_vehicle = null; - - if (!pos.IsFinite()) - { - pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), - parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); - } - _position = pos; - givefakepos = 0; - - PID_D = parent_scene.bodyPIDD; - PID_G = parent_scene.bodyPIDG; - m_density = parent_scene.geomDefaultDensity; - // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; - body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - - prim_geom = IntPtr.Zero; - collide_geom = IntPtr.Zero; - Body = IntPtr.Zero; - - if (!size.IsFinite()) - { - size = new Vector3(0.5f, 0.5f, 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); - } - - if (size.X <= 0) size.X = 0.01f; - if (size.Y <= 0) size.Y = 0.01f; - if (size.Z <= 0) size.Z = 0.01f; - - _size = size; - - if (!QuaternionIsFinite(rotation)) - { - rotation = Quaternion.Identity; - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); - } - - _orientation = rotation; - givefakeori = 0; - - _pbs = pbs; - - _parent_scene = parent_scene; - m_targetSpace = IntPtr.Zero; - - if (pos.Z < 0) - { - m_isphysical = false; - } - else - { - m_isphysical = pisPhysical; - } - m_fakeisphysical = m_isphysical; - - m_isVolumeDetect = false; - - m_force = Vector3.Zero; - - m_iscolliding = false; - m_colliderfilter = 0; - m_softcolide = true; - m_NoColide = false; - - hasOOBoffsetFromMesh = false; - _triMeshData = IntPtr.Zero; - - m_shapetype = _shapeType; - - m_lastdoneSelected = false; - m_isSelected = false; - m_delaySelect = false; - - m_isphantom = pisPhantom; - m_fakeisphantom = pisPhantom; - - mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; - bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; - - CalcPrimBodyData(); - - m_building = true; // control must set this to false when done - - AddChange(changes.Add, null); - } - - private void resetCollisionAccounting() - { - m_collisionscore = 0; - } - - private void createAMotor(Vector3 axis) - { - if (Body == IntPtr.Zero) - return; - - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - - int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); - - if (axisnum <= 0) - return; - - // stop it - d.BodySetTorque(Body, 0, 0, 0); - d.BodySetAngularVel(Body, 0, 0, 0); - - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - - d.JointSetAMotorMode(Amotor, 0); - - d.JointSetAMotorNumAxes(Amotor, axisnum); - - // get current orientation to lock - - d.Quaternion dcur = d.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; - Vector3 ax; - - int i = 0; - int j = 0; - if (axis.X == 0) - { - ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X - // ODE should do this with axis relative to body 1 but seems to fail - d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, 0, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); - i++; - j = 256; // move to next axis set - } - - if (axis.Y == 0) - { - ax = (new Vector3(0, 1, 0)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - i++; - j += 256; - } - - if (axis.Z == 0) - { - ax = (new Vector3(0, 0, 1)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - } - } - - private bool setMesh(OdeScene parent_scene) - { - if (Body != IntPtr.Zero) - { - if (childPrim) - { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this, false); - } - } - else - { - DestroyBody(); - } - } - - bool convex; - if (m_shapetype == 0) - convex = false; - else - convex = true; - - IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex); - if (mesh == null) - { - m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); - return false; - } - - IntPtr vertices, indices; - int vertexCount, indexCount; - int vertexStride, triStride; - - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage - - if (vertexCount == 0 || indexCount == 0) - { - m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", - Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); - mesh.releaseSourceMeshData(); - return false; - } - - primOOBoffset = mesh.GetCentroid(); - hasOOBoffsetFromMesh = true; - - mesh.releaseSourceMeshData(); - - IntPtr geo = IntPtr.Zero; - - try - { - _triMeshData = d.GeomTriMeshDataCreate(); - - d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); - d.GeomTriMeshDataPreprocess(_triMeshData); - - _parent_scene.waitForSpaceUnlock(m_targetSpace); - geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); - } - - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - return false; - } - - SetGeom(geo); - return true; - } - - private void SetGeom(IntPtr geom) - { - prim_geom = geom; - //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - { - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCollideBits(prim_geom, 0); - d.GeomDisable(prim_geom); - } - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - - CalcPrimBodyData(); - - _parent_scene.geom_name_map[prim_geom] = Name; - _parent_scene.actor_name_map[prim_geom] = this; - - } - else - m_log.Warn("Setting bad Geom"); - } - - - /// - /// Create a geometry for the given mesh in the given target space. - /// - /// - /// If null, then a mesh is used that is based on the profile shape data. - private void CreateGeom() - { - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - - bool haveMesh = false; - hasOOBoffsetFromMesh = false; - m_NoColide = false; - - if (_parent_scene.needsMeshing(_pbs)) - { - haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims - if (!haveMesh) - m_NoColide = true; - } - - if (!haveMesh) - { - if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 - && _size.X == _size.Y && _size.Y == _size.Z) - { // it's a sphere - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); - return; - } - } - else - {// do it as a box - _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - //Console.WriteLine(" CreateGeom 4"); - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); - } - catch (Exception e) - { - m_log.Warn("[PHYSICS]: Create box failed: {0}", e); - return; - } - } - } - } - - /// - /// Set a new geometry for this prim. - /// - /// - private void RemoveGeom() - { - if (prim_geom != IntPtr.Zero) - { - _parent_scene.geom_name_map.Remove(prim_geom); - _parent_scene.actor_name_map.Remove(prim_geom); - try - { - d.GeomDestroy(prim_geom); - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - } - // catch (System.AccessViolationException) - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); - } - - prim_geom = IntPtr.Zero; - } - else - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); - } - Body = IntPtr.Zero; - hasOOBoffsetFromMesh = false; - CalcPrimBodyData(); - } - - private void ChildSetGeom(OdePrim odePrim) - { - // well.. - DestroyBody(); - MakeBody(); - } - - //sets non physical prim m_targetSpace to right space in spaces grid for static prims - // should only be called for non physical prims unless they are becoming non physical - private void SetInStaticSpace(OdePrim prim) - { - IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); - prim.m_targetSpace = targetSpace; - d.GeomEnable(prim_geom); - } - - public void enableBodySoft() - { - if (!childPrim && !m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - m_collisionFlags = CollisionCategories.Land; - } - else - { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prm.prim_geom); - } - } - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prim_geom); - } - d.BodyEnable(Body); - } - } - m_disabled = false; - resetCollisionAccounting(); // this sets m_disable to false - } - - private void disableBodySoft() - { - m_disabled = true; - if (!childPrim) - { - if (m_isphysical && Body != IntPtr.Zero) - { - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prm.prim_geom); - } - } - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prim_geom); - } - - d.BodyDisable(Body); - } - } - } - - private void MakeBody() - { - if (!m_isphysical) // only physical get bodies - return; - - if (childPrim) // child prims don't get bodies; - return; - - if (m_building) - return; - - if (prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); - return; - } - - if (Body != IntPtr.Zero) - { - d.BodyDestroy(Body); - Body = IntPtr.Zero; - m_log.Warn("[PHYSICS]: MakeBody called having a body"); - } - - - if (d.GeomGetBody(prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); - } - - d.Matrix3 mymat = new d.Matrix3(); - d.Quaternion myrot = new d.Quaternion(); - d.Mass objdmass = new d.Mass { }; - - Body = d.BodyCreate(_parent_scene.world); - - DMassDup(ref primdMass, out objdmass); - - // rotate inertia - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); - - // set the body rotation - d.BodySetRotation(Body, ref mymat); - - // recompute full object inertia if needed - if (childrenPrim.Count > 0) - { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - d.Mass tmpdmass = new d.Mass { }; - Vector3 rcm; - - rcm.X = _position.X + objdmass.c.X; - rcm.Y = _position.Y + objdmass.c.Y; - rcm.Z = _position.Z + objdmass.c.Z; - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); - continue; - } - - DMassCopy(ref prm.primdMass, ref tmpdmass); - - // apply prim current rotation to inertia - quat.X = prm._orientation.X; - quat.Y = prm._orientation.Y; - quat.Z = prm._orientation.Z; - quat.W = prm._orientation.W; - d.RfromQ(out mat, ref quat); - d.MassRotate(ref tmpdmass, ref mat); - - Vector3 ppos = prm._position; - ppos.X += tmpdmass.c.X - rcm.X; - ppos.Y += tmpdmass.c.Y - rcm.Y; - ppos.Z += tmpdmass.c.Z - rcm.Z; - - // refer inertia to root prim center of mass position - d.MassTranslate(ref tmpdmass, - ppos.X, - ppos.Y, - ppos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia - // fix prim colision cats - - if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); - } - - d.GeomClearOffset(prm.prim_geom); - d.GeomSetBody(prm.prim_geom, Body); - prm.Body = Body; - d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation - } - } - } - - d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset - // associate root geom with body - d.GeomSetBody(prim_geom, Body); - - d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); - d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); - - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - myrot.W = -myrot.W; - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; - - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode(Body, false); - - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - // d.BodySetLinearDampingThreshold(Body, 0.01f); - // d.BodySetAngularDampingThreshold(Body, 0.001f); - d.BodySetDamping(Body, .002f, .002f); - - - if (m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - if (d.SpaceQuery(m_targetSpace, prim_geom)) - d.SpaceRemove(m_targetSpace, prim_geom); - } - - - if (childrenPrim.Count == 0) - { - collide_geom = prim_geom; - m_targetSpace = _parent_scene.ActiveSpace; - d.SpaceAdd(m_targetSpace, prim_geom); - } - else - { - m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); - d.HashSpaceSetLevels(m_targetSpace, -2, 8); - d.SpaceSetSublevel(m_targetSpace, 3); - d.SpaceSetCleanup(m_targetSpace, false); - d.SpaceAdd(m_targetSpace, prim_geom); - collide_geom = m_targetSpace; - } - - if (m_delaySelect) - { - m_isSelected = true; - m_delaySelect = false; - } - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom == IntPtr.Zero) - continue; - - Vector3 ppos = prm._position; - d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position - - if (prm.m_targetSpace != m_targetSpace) - { - if (prm.m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); - if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) - d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); - } - prm.m_targetSpace = m_targetSpace; - d.SpaceAdd(m_targetSpace, prm.prim_geom); - } - - if (m_isSelected || m_disabled) - { - prm.m_collisionCategories &= ~CollisionCategories.Body; - prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); - d.GeomDisable(prm.prim_geom); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - prm.m_collisionCategories = 0; - prm.m_collisionFlags = CollisionCategories.Land; - } - else - { - prm.m_collisionCategories |= CollisionCategories.Body; - prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - d.GeomEnable(prm.prim_geom); - } - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - d.GeomEnable(prm.prim_geom); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); - } - prm.m_collisionscore = 0; - - if(!m_disabled) - prm.m_disabled = false; - - _parent_scene.addActivePrim(prm); - } - } - - // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) - { - createAMotor(m_angularlock); - } - - if (m_isSelected || m_disabled) - { - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); - - d.GeomDisable(prim_geom); - d.BodyDisable(Body); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - m_collisionFlags = CollisionCategories.Land; - } - else - { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - - d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - } - - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - - m_collisionscore = 0; - - m_softcolide = true; - _parent_scene.addActivePrim(this); - _parent_scene.addActiveGroups(this); - } - - private void DestroyBody() - { - if (Body != IntPtr.Zero) - { - _parent_scene.remActivePrim(this); - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - UpdateDataFromGeom(); - d.GeomSetBody(prim_geom, IntPtr.Zero); - SetInStaticSpace(this); - } - - if (!childPrim) - { - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - _parent_scene.remActivePrim(prm); - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - prm.UpdateDataFromGeom(); - SetInStaticSpace(prm); - } - prm.Body = IntPtr.Zero; - prm._mass = prm.primMass; - prm.m_collisionscore = 0; - } - } - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - _parent_scene.remActiveGroup(this); - d.BodyDestroy(Body); - } - Body = IntPtr.Zero; - } - _mass = primMass; - m_collisionscore = 0; - } - - private void FixInertia(Vector3 NewPos,Quaternion newrot) - { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - - d.BodyGetMass(Body, out tmpdmass); - objdmass = tmpdmass; - - d.Vector3 dobjpos; - d.Vector3 thispos; - - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); - - // get prim own inertia in its local frame - tmpdmass = primdMass; - - // transform to object frame - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); - - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); - - // back prim own inertia - tmpdmass = primdMass; - - // update to new position and orientation - _position = NewPos; - d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); - _orientation = newrot; - quat.X = newrot.X; - quat.Y = newrot.Y; - quat.Z = newrot.Z; - quat.W = newrot.W; - d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); - - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); - - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) - { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); + protected bool m_forcePosOrRotation; + + private Quaternion m_lastorientation = new Quaternion(); + private Quaternion _orientation; + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _torque; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 _acceleration; + private Vector3 m_angularlock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; + + private Vector3 m_force; + private Vector3 m_forceacc; + private Vector3 m_angularForceacc; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private float PID_D = 35f; + private float PID_G = 25f; + private bool m_usePID; + + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private float m_targetHoverHeight; + private float m_groundHeight; + private float m_waterHeight; + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + + private int body_autodisable_frames = 20; + + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); +// private bool m_collidesLand = true; + private bool m_collidesWater; + public bool m_returnCollisions; + private bool m_softcolide; + + private bool m_NoColide; // for now only for internal use for bad meshs + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlags; + + public bool m_disabled; + + public uint m_localID; + + private IMesh m_mesh; + private object m_meshlock = new object(); + private PrimitiveBaseShape _pbs; + public OdeScene _parent_scene; + + /// + /// The physics space which contains prim geometry + /// + public IntPtr m_targetSpace = IntPtr.Zero; + + public IntPtr prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent; + + private List childrenPrim = new List(); + + private bool m_iscolliding; + + public bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + public bool m_outbounds; + + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + private bool m_throttleUpdates; + private int throttleCounter; + public float m_collisionscore; + int m_colliderfilter = 0; + + public IntPtr collide_geom; // for objects: geom if single prim space it linkset + + private float m_density = 10.000006836f; // Aluminum g/cm3; + private byte m_shapetype; + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body = IntPtr.Zero; + public String Name { get; private set; } + private Vector3 _target_velocity; + + public Vector3 primOOBsize; // prim real dimensions from mesh + public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb + public float primOOBradiusSQ; + public d.Mass primdMass; // prim inertia information on it's own referencial + float primMass; // prim own mass + float _mass; // object mass acording to case + private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb + + public int givefakepos = 0; + private Vector3 fakepos; + public int givefakeori = 0; + private Quaternion fakeori; + + public int m_eventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + public volatile bool childPrim; + + public ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + private float mu; + private float bounce; + + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical // this is not reliable for internal use + { + get { return m_fakeisphysical; } + set + { + m_fakeisphysical = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + if (!value) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + AddChange(changes.Physical, value); } - d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); + } + + public override bool Phantom // this is not reliable for internal use + { + get { return m_fakeisphantom; } + set + { + m_fakeisphantom = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + AddChange(changes.Phantom, value); + } + } - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; + public override bool Building // this is not reliable for internal use + { + get { return m_building; } + set + { + if (value) + m_building = true; + AddChange(changes.building, value); + } } + public override void getContactData(ref ContactData cdata) + { + cdata.mu = mu; + cdata.bounce = bounce; + // cdata.softcolide = m_softcolide; + cdata.softcolide = false; - private void FixInertia(Vector3 NewPos) + if (m_isphysical) + { + ODEDynamics veh; + if (_parent != null) + veh = ((OdePrim)_parent).m_vehicle; + else + veh = m_vehicle; + + if (veh != null && veh.Type != Vehicle.TYPE_NONE) + cdata.mu *= veh.FrictionFactor; + } + } + + public override int PhysicsActorType { - d.Matrix3 primmat = new d.Matrix3(); - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - d.Mass primmass = new d.Mass { }; + get { return (int)ActorTypes.Prim; } + set { return; } + } - d.Vector3 dobjpos; - d.Vector3 thispos; + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } - d.BodyGetMass(Body, out objdmass); + public override uint LocalID + { + get + { + return m_localID; + } + set + { + //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); + m_localID = value; + } + } - // get prim own inertia in its local frame - primmass = primdMass; - // transform to object frame - primmat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref primmass, ref primmat); + public override bool Grabbed + { + set { return; } + } - tmpdmass = primmass; + public override bool Selected + { + set + { + if (value) + m_isSelected = value; // if true set imediatly to stop moves etc + AddChange(changes.Selected, value); + } + } - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } - // update to new position - _position = NewPos; - d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + if (m_colliderfilter == 0) + { + m_softcolide = false; + m_iscolliding = false; + } + else + m_iscolliding = true; + } + } - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref primmass, - thispos.X, - thispos.Y, - thispos.Z); + public override bool CollidingGround + { + get { return false; } + set { return; } + } - d.MassAdd(ref objdmass, ref primmass); + public override bool CollidingObj + { + get { return false; } + set { return; } + } - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); + if (givefakepos > 0) + return fakepos; + else + return _position; } - d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + set + { + fakepos = value; + givefakepos++; + AddChange(changes.Position, value); + } + } - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); + public override Vector3 Size + { + get { return _size; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Size, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + } + } + } - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; + public override float Mass + { + get { return _mass; } } - private void FixInertia(Quaternion newrot) + public override Vector3 Force { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); + //get { return Vector3.Zero; } + get { return m_force; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Force, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + } + } + } - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - d.Vector3 dobjpos; - d.Vector3 thispos; + public override void SetVolumeDetect(int param) + { + AddChange(changes.VolumeDtc, (param != 0)); + } - d.BodyGetMass(Body, out objdmass); + public override Vector3 GeometricCenter + { + get + { + return Vector3.Zero; + } + } - // get prim own inertia in its local frame - tmpdmass = primdMass; - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - // transform to object frame - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); + public override Vector3 CenterOfMass + { + get + { + d.Vector3 dtmp; + if (IsPhysical && !childPrim && Body != IntPtr.Zero) + { + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); + Vector3 vtmp = primOOBoffset * q; + dtmp = d.GeomGetPosition(prim_geom); + return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); + } + else + return Vector3.Zero; + } + } + /* + public override Vector3 PrimOOBsize + { + get + { + return primOOBsize; + } + } - // update to new orientation - _orientation = newrot; - quat.X = newrot.X; - quat.Y = newrot.Y; - quat.Z = newrot.Z; - quat.W = newrot.W; - d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + public override Vector3 PrimOOBoffset + { + get + { + return primOOBoffset; + } + } - tmpdmass = primdMass; - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); + public override float PrimOOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } + } + */ + public override PrimitiveBaseShape Shape + { + set + { +/* + IMesh mesh = null; + if (_parent_scene.needsMeshing(value)) + { + bool convex; + if (m_shapetype == 0) + convex = false; + else + convex = true; + mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex); + } - d.MassAdd(ref objdmass, ref tmpdmass); + if (mesh != null) + { + lock (m_meshlock) + m_mesh = mesh; + } +*/ + AddChange(changes.Shape, value); + } + } - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) + public override byte PhysicsShapeType + { + get { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); + return m_shapetype; + } + set + { + m_shapetype = value; + AddChange(changes.Shape, null); } + } - d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; + public override Vector3 Velocity + { + get + { + if (_zeroFlag) + return Vector3.Zero; + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); + // _velocity = value; + + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); + } + + } } - - - #region Mass Calculation - - private float CalculatePrimVolume() - { - float volume = _size.X * _size.Y * _size.Z; // default - float tmp; - - float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - - switch (_pbs.ProfileShape) - { - case ProfileShape.Square: - // default box - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - - hollowVolume *= (0.5f * .5f); - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - //a tube - - volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume * tmp * tmp; - - if (hollowAmount > 0.0) - { - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - hollowVolume *= 0.5f * 0.5f; - break; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - break; - - case ProfileShape.Circle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.78539816339f; // elipse base - - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - case ProfileShape.HalfCircle: - if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.52359877559829887307710723054658f; - } - break; - - case ProfileShape.EquilateralTriangle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.32475953f; - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.32475953f; - volume *= 0.01f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - default: - break; - } - - float taperX1; - float taperY1; - float taperX; - float taperY; - float pathBegin; - float pathEnd; - float profileBegin; - float profileEnd; - - if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) - { - taperX1 = _pbs.PathScaleX * 0.01f; - if (taperX1 > 1.0f) - taperX1 = 2.0f - taperX1; - taperX = 1.0f - taperX1; - - taperY1 = _pbs.PathScaleY * 0.01f; - if (taperY1 > 1.0f) - taperY1 = 2.0f - taperY1; - taperY = 1.0f - taperY1; - } - else - { - taperX = _pbs.PathTaperX * 0.01f; - if (taperX < 0.0f) - taperX = -taperX; - taperX1 = 1.0f - taperX; - - taperY = _pbs.PathTaperY * 0.01f; - if (taperY < 0.0f) - taperY = -taperY; - taperY1 = 1.0f - taperY; - } - - volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); - - pathBegin = (float)_pbs.PathBegin * 2.0e-5f; - pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; - volume *= (pathEnd - pathBegin); - - // this is crude aproximation - profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; - profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; - volume *= (profileEnd - profileBegin); - - return volume; - } - - - private void CalcPrimBodyData() - { - float volume; - - if (prim_geom == IntPtr.Zero) - { - // Ubit let's have a initial basic OOB - primOOBsize.X = _size.X; - primOOBsize.Y = _size.Y; - primOOBsize.Z = _size.Z; - primOOBoffset = Vector3.Zero; - } - else - { - d.AABB AABB; - d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom - - primOOBsize.X = (AABB.MaxX - AABB.MinX); - primOOBsize.Y = (AABB.MaxY - AABB.MinY); - primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); - if (!hasOOBoffsetFromMesh) - { - primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; - primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; - primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; - } - } - - // also its own inertia and mass - // keep using basic shape mass for now - volume = CalculatePrimVolume(); - - primMass = m_density * volume; - - if (primMass <= 0) - primMass = 0.0001f;//ckrinke: Mass must be greater then zero. - if (primMass > _parent_scene.maximumMassObject) - primMass = _parent_scene.maximumMassObject; - - _mass = primMass; // just in case - - d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); - - d.MassTranslate(ref primdMass, - primOOBoffset.X, - primOOBoffset.Y, - primOOBoffset.Z); - - primOOBsize *= 0.5f; // let obb size be a corner coords - primOOBradiusSQ = primOOBsize.LengthSquared(); - } - - - #endregion - - - /// - /// Add a child prim to this parent prim. - /// - /// Child prim - // I'm the parent - // prim is the child - public void ParentPrim(OdePrim prim) - { - //Console.WriteLine("ParentPrim " + m_primName); - if (this.m_localID != prim.m_localID) - { - DestroyBody(); // for now we need to rebuil entire object on link change - - lock (childrenPrim) - { - // adopt the prim - if (!childrenPrim.Contains(prim)) - childrenPrim.Add(prim); - - // see if this prim has kids and adopt them also - // should not happen for now - foreach (OdePrim prm in prim.childrenPrim) - { - if (!childrenPrim.Contains(prm)) - { - if (prm.Body != IntPtr.Zero) - { - if (prm.prim_geom != IntPtr.Zero) - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - if (prm.Body != prim.Body) - prm.DestroyBody(); // don't loose bodies around - prm.Body = IntPtr.Zero; - } - - childrenPrim.Add(prm); - prm._parent = this; - } - } - } - //Remove old children from the prim - prim.childrenPrim.Clear(); - - if (prim.Body != IntPtr.Zero) - { - if (prim.prim_geom != IntPtr.Zero) - d.GeomSetBody(prim.prim_geom, IntPtr.Zero); - prim.DestroyBody(); // don't loose bodies around - prim.Body = IntPtr.Zero; - } - - prim.childPrim = true; - prim._parent = this; - - MakeBody(); // full nasty reconstruction - } - } - - private void UpdateChildsfromgeom() - { - if (childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.UpdateDataFromGeom(); - } - } - - private void UpdateDataFromGeom() - { - if (prim_geom != IntPtr.Zero) - { - d.Quaternion qtmp = new d.Quaternion { }; - d.GeomCopyQuaternion(prim_geom, out qtmp); - _orientation.W = qtmp.W; - _orientation.X = qtmp.X; - _orientation.Y = qtmp.Y; - _orientation.Z = qtmp.Z; - - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - } - } - - private void ChildDelink(OdePrim odePrim, bool remakebodies) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) // delinking the root prim - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) - { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); - } - if (newroot != null) - { - newroot.childPrim = false; - newroot._parent = null; - if (remakebodies) - newroot.MakeBody(); - } - } - } - - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - // odePrim.UpdateDataFromGeom(); - if (remakebodies) - odePrim.MakeBody(); - } - } - if (remakebodies) - MakeBody(); - } - - protected void ChildRemove(OdePrim odePrim, bool reMakeBody) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) - { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); - } - if (newroot != null) - { - newroot.childPrim = false; - newroot._parent = null; - newroot.MakeBody(); - } - } - if (reMakeBody) - MakeBody(); - return; - } - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - if (reMakeBody) - odePrim.MakeBody(); - } - } - MakeBody(); - } - - #region changes - - private void changeadd() - { - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - - if (!m_isphysical) - SetInStaticSpace(this); - } - - if (m_isphysical && Body == IntPtr.Zero) - { - MakeBody(); - } - } - - private void changeAngularLock(Vector3 newLock) - { - // do we have a Physical object? - if (Body != IntPtr.Zero) - { - //Check that we have a Parent - //If we have a parent then we're not authorative here - if (_parent == null) - { - if (!newLock.ApproxEquals(Vector3.One, 0f)) - { - createAMotor(newLock); - } - else - { - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - } - } - } - // Store this for later in case we get turned into a separate body - m_angularlock = newLock; - } - - private void changeLink(OdePrim NewParent) - { - if (_parent == null && NewParent != null) - { - NewParent.ParentPrim(this); - } - else if (_parent != null) - { - if (_parent is OdePrim) - { - if (NewParent != _parent) - { - (_parent as OdePrim).ChildDelink(this, false); // for now... - childPrim = false; - - if (NewParent != null) - { - NewParent.ParentPrim(this); - } - } - } - } - _parent = NewParent; - } - - - private void Stop() - { - if (!childPrim) - { - m_force = Vector3.Zero; - m_forceacc = Vector3.Zero; - m_angularForceacc = Vector3.Zero; - _torque = Vector3.Zero; - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - _target_velocity = Vector3.Zero; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - } - - if (Body != IntPtr.Zero) - { - d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetTorque(Body, 0f, 0f, 0f); - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetAngularVel(Body, 0f, 0f, 0f); - } - } - - - private void changePhantomStatus(bool newval) - { - m_isphantom = newval; - - if (m_isSelected) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - if (m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; // should never happen - } - - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; - } - } - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - if(!m_isSelected) - d.GeomEnable(prm.prim_geom); - } - } - } - - if (!m_disabled && prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - if(!m_isSelected) - d.GeomEnable(prim_geom); - } - } - - private void changeSelectedStatus(bool newval) - { - if (m_lastdoneSelected == newval) - return; - - m_lastdoneSelected = newval; - DoSelectedStatus(newval); - } - - private void CheckDelaySelect() - { - if (m_delaySelect) - { - DoSelectedStatus(m_isSelected); - } - } - - private void DoSelectedStatus(bool newval) - { - m_isSelected = newval; - Stop(); - - if (newval) - { - if (!childPrim && Body != IntPtr.Zero) - d.BodyDisable(Body); - - if (m_delaySelect || m_isphysical) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != null) - { - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prm.prim_geom); - } - prm.m_delaySelect = false; - } - } - - if (prim_geom != null) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prim_geom); - } - - m_delaySelect = false; - } - else if(!m_isphysical) - { - m_delaySelect = true; - } - } - else - { - if (!childPrim && Body != IntPtr.Zero && !m_disabled) - d.BodyEnable(Body); - - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - if(m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; - } - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; - } - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prm.prim_geom); - } - prm.m_delaySelect = false; - prm.m_softcolide = true; - } - } - - if (!m_disabled && prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prim_geom); - } - - m_delaySelect = false; - m_softcolide = true; - } - - resetCollisionAccounting(); - } - - private void changePosition(Vector3 newPos) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _position = newPos; - } - - else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) - { - FixInertia(newPos); - if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (prim_geom != IntPtr.Zero) - { - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } - } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; - // changeSelectedStatus(); - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changeOrientation(Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _orientation = newOri; - } - /* - else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) - { - FixInertia(_position, newOri); - if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - */ - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } - } - } - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim && m_building) // inertia is messed, must rebuild - { - _position = newPos; - _orientation = newOri; - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); - // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } - - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } - } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - - m_softcolide = true; - resetCollisionAccounting(); - } - - - private void changeDisable(bool disable) - { - if (disable) - { - if (!m_disabled) - disableBodySoft(); - } - else - { - if (m_disabled) - enableBodySoft(); - } - } - - private void changePhysicsStatus(bool NewStatus) - { - CheckDelaySelect(); - - m_isphysical = NewStatus; - - if (!childPrim) - { - if (NewStatus) - { - if (Body == IntPtr.Zero) - MakeBody(); - } - else - { - if (Body != IntPtr.Zero) - { - DestroyBody(); - } - Stop(); - } - } - - resetCollisionAccounting(); - } - - private void changeprimsizeshape() - { - CheckDelaySelect(); - - OdePrim parent = (OdePrim)_parent; - - bool chp = childPrim; - - if (chp) - { - if (parent != null) - { - parent.DestroyBody(); - } - } - else - { - DestroyBody(); - } - - RemoveGeom(); - - // we don't need to do space calculation because the client sends a position update also. - if (_size.X <= 0) - _size.X = 0.01f; - if (_size.Y <= 0) - _size.Y = 0.01f; - if (_size.Z <= 0) - _size.Z = 0.01f; - // Construction of new prim - - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - } - - if (chp) - { - if (parent != null) - { - parent.MakeBody(); - } - } - else - MakeBody(); - - m_softcolide = true; - resetCollisionAccounting(); - } - - private void changeSize(Vector3 newSize) - { - _size = newSize; - changeprimsizeshape(); - } - - private void changeShape(PrimitiveBaseShape newShape) - { - if(newShape != null) - _pbs = newShape; - changeprimsizeshape(); - } - - private void changeFloatOnWater(bool newval) - { - m_collidesWater = newval; - - if (prim_geom != IntPtr.Zero && !m_isphantom) - { - if (m_collidesWater) - { - m_collisionFlags |= CollisionCategories.Water; - } - else - { - m_collisionFlags &= ~CollisionCategories.Water; - } - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - } - - private void changeSetTorque(Vector3 newtorque) - { - if (!m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - } - _torque = newtorque; - } - } - - private void changeForce(Vector3 force) - { - m_force = force; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - - private void changeAddForce(Vector3 force) - { - m_forceacc += force; - if (!m_isSelected) - { - lock (this) - { - //m_log.Info("[PHYSICS]: dequeing forcelist"); - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - - m_collisionscore = 0; - } - } - - private void changeAddAngularForce(Vector3 aforce) - { - m_angularForceacc += aforce; - if (!m_isSelected) - { - lock (this) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - m_collisionscore = 0; - } - } - - private void changevelocity(Vector3 newVel) - { - if (!m_isSelected) - { - if (Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); - } - //resetCollisionAccounting(); - } - _velocity = newVel; - } - - private void changeVolumedetetion(bool newVolDtc) - { - m_isVolumeDetect = newVolDtc; - } - - protected void changeBuilding(bool newbuilding) - { - if ((bool)newbuilding) - { - m_building = true; - if (!childPrim) - DestroyBody(); - } - else - { - m_building = false; - CheckDelaySelect(); - if (!childPrim) - MakeBody(); - } - if (!childPrim && childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.changeBuilding(m_building); // call directly - } - } - - public void changeSetVehicle(VehicleData vdata) - { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - m_vehicle.DoSetVehicle(vdata); - } - private void changeVehicleType(int value) - { - if (value == (int)Vehicle.TYPE_NONE) - { - if (m_vehicle != null) - m_vehicle = null; - } - else - { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - - m_vehicle.ProcessTypeChange((Vehicle)value); - } - } - - private void changeVehicleFloatParam(strVehicleFloatParam fp) - { - if (m_vehicle == null) - return; - - m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); - } - - private void changeVehicleVectorParam(strVehicleVectorParam vp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); - } - - private void changeVehicleRotationParam(strVehicleQuatParam qp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); - } - - private void changeVehicleFlags(strVehicleBoolParam bp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVehicleFlags(bp.param, bp.value); - } - - #endregion - - public void Move() - { - if (!childPrim && m_isphysical && Body != IntPtr.Zero && - !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) - // !m_disabled && !m_isSelected && !m_building && !m_outbounds) - { -// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 - - float timestep = _parent_scene.ODE_STEPSIZE; - - // check outside region - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - - if (lpos.Z < -100 || lpos.Z > 100000f) - { - m_outbounds = true; - - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - return; - } - - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if(lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if(lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } - - if(m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; - - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - base.RequestPhysicsterseUpdate(); - return; - } - - - float fx = 0; - float fy = 0; - float fz = 0; - - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(); - } - else - { - float m_mass = _mass; - - // fz = 0f; - //m_log.Info(m_collisionFlags.ToString()); - if (m_usePID) - { - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } // end if (m_usePID) - - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID) - { - //Console.WriteLine("Hover " + Name); - - // If we're using the PID controller, then we have no gravity - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; - - } // end switch (m_PIDHoverType) - - - _target_velocity = - new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - // ? d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } - else - { - float b = (1.0f - m_buoyancy); - fx = _parent_scene.gravityx * b; - fy = _parent_scene.gravityy * b; - fz = _parent_scene.gravityz * b; - } - - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; - - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; - - m_forceacc = Vector3.Zero; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - d.BodyAddForce(Body, fx, fy, fz); - //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - - Vector3 trq; - - trq = _torque; - trq += m_angularForceacc; - m_angularForceacc = Vector3.Zero; - if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) - { - d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); - } - - } - } - else - { // is not physical, or is not a body or is selected - // _zeroPosition = d.BodyGetPosition(Body); - return; - //Console.WriteLine("Nothing " + Name); - - } - } - - - public void UpdatePositionAndVelocity(float simulatedtime) - { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null && !m_disabled && !m_building && !m_outbounds) - { - if (Body != IntPtr.Zero) - { - Vector3 pv = Vector3.Zero; - bool lastZeroFlag = _zeroFlag; - - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - - - d.Quaternion ori; - d.GeomCopyQuaternion(prim_geom, out ori); - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 rotvel = d.BodyGetAngularVel(Body); - - if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) - && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) - && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) - && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) - && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) - && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) - ) - { - _zeroFlag = true; - //Console.WriteLine("ZFT 2"); - m_throttleUpdates = false; - } - else - { - //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); - _zeroFlag = false; - m_lastUpdateSent = false; - //m_throttleUpdates = false; - } - - if (_zeroFlag) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - m_rotationalVelocity = pv; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; - } - } - else - { - if (lastZeroFlag != _zeroFlag) - { - base.RequestPhysicsterseUpdate(); - } - - m_lastVelocity = _velocity; - - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - - _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - { - m_rotationalVelocity = pv; - } - else - { - m_rotationalVelocity.X = rotvel.X; - m_rotationalVelocity.Y = rotvel.Y; - m_rotationalVelocity.Z = rotvel.Z; - } - - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) - { - m_lastposition = _position; - m_lastorientation = _orientation; - base.RequestPhysicsterseUpdate(); - } - else - { - throttleCounter++; - } - } - } - else if (!m_lastUpdateSent || !_zeroFlag) - { - // Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; - - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; - } - } - } - } - - internal static bool QuaternionIsFinite(Quaternion q) - { - if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) - return false; - if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) - return false; - if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) - return false; - if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) - return false; - return true; - } - - internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) - { - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } - - internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) - { - // assumes object center of mass is zero - float smass = part.mass; - theobj.mass -= smass; - - smass *= 1.0f / (theobj.mass); ; - - theobj.c.X -= part.c.X * smass; - theobj.c.Y -= part.c.Y * smass; - theobj.c.Z -= part.c.Z * smass; - - theobj.I.M00 -= part.I.M00; - theobj.I.M01 -= part.I.M01; - theobj.I.M02 -= part.I.M02; - theobj.I.M10 -= part.I.M10; - theobj.I.M11 -= part.I.M11; - theobj.I.M12 -= part.I.M12; - theobj.I.M20 -= part.I.M20; - theobj.I.M21 -= part.I.M21; - theobj.I.M22 -= part.I.M22; - } - - private static void DMassDup(ref d.Mass src, out d.Mass dst) - { - dst = new d.Mass { }; - - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } - private void donullchange() - { - } - - public bool DoAChange(changes what, object arg) - { - if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) - { - return false; - } - - // nasty switch - switch (what) - { - case changes.Add: - changeadd(); - break; - case changes.Remove: - //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... - //When we return true, it destroys all of the prims in the linkset anyway - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildRemove(this, false); - } - else - ChildRemove(this, false); - - m_vehicle = null; - RemoveGeom(); - m_targetSpace = IntPtr.Zero; - if (m_eventsubscription > 0) - UnSubscribeEvents(); - return true; - - case changes.Link: - OdePrim tmp = (OdePrim)arg; - changeLink(tmp); - break; - - case changes.DeLink: - changeLink(null); - break; - - case changes.Position: - changePosition((Vector3)arg); - break; - - case changes.Orientation: - changeOrientation((Quaternion)arg); - break; - - case changes.PosOffset: - donullchange(); - break; - - case changes.OriOffset: - donullchange(); - break; - - case changes.Velocity: - changevelocity((Vector3)arg); - break; - - // case changes.Acceleration: - // changeacceleration((Vector3)arg); - // break; - // case changes.AngVelocity: - // changeangvelocity((Vector3)arg); - // break; - - case changes.Force: - changeForce((Vector3)arg); - break; - - case changes.Torque: - changeSetTorque((Vector3)arg); - break; - - case changes.AddForce: - changeAddForce((Vector3)arg); - break; - - case changes.AddAngForce: - changeAddAngularForce((Vector3)arg); - break; - - case changes.AngLock: - changeAngularLock((Vector3)arg); - break; - - case changes.Size: - changeSize((Vector3)arg); - break; - - case changes.Shape: - changeShape((PrimitiveBaseShape)arg); - break; - - case changes.CollidesWater: - changeFloatOnWater((bool)arg); - break; - - case changes.VolumeDtc: - changeVolumedetetion((bool)arg); - break; - - case changes.Phantom: - changePhantomStatus((bool)arg); - break; - - case changes.Physical: - changePhysicsStatus((bool)arg); - break; - - case changes.Selected: - changeSelectedStatus((bool)arg); - break; - - case changes.disabled: - changeDisable((bool)arg); - break; - - case changes.building: - changeBuilding((bool)arg); - break; - - case changes.VehicleType: - changeVehicleType((int)arg); - break; - - case changes.VehicleFlags: - changeVehicleFlags((strVehicleBoolParam) arg); - break; - - case changes.VehicleFloatParam: - changeVehicleFloatParam((strVehicleFloatParam) arg); - break; - - case changes.VehicleVectorParam: - changeVehicleVectorParam((strVehicleVectorParam) arg); - break; - - case changes.VehicleRotationParam: - changeVehicleRotationParam((strVehicleQuatParam) arg); - break; - - case changes.SetVehicle: - changeSetVehicle((VehicleData) arg); - break; - case changes.Null: - donullchange(); - break; - - default: - donullchange(); - break; - } - return false; - } - - public void AddChange(changes what, object arg) - { - _parent_scene.AddChange((PhysicsActor) this, what, arg); - } - - - private struct strVehicleBoolParam - { - public int param; - public bool value; - } - - private struct strVehicleFloatParam - { - public int param; - public float value; - } - - private struct strVehicleQuatParam - { - public int param; - public Quaternion value; - } - - private struct strVehicleVectorParam - { - public int param; - public Vector3 value; - } - } + + public override Vector3 Torque + { + get + { + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return _torque; + } + + set + { + if (value.IsFinite()) + { + AddChange(changes.Torque, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); + } + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get + { + if (givefakeori > 0) + return fakeori; + else + + return _orientation; + } + set + { + if (QuaternionIsFinite(value)) + { + fakeori = value; + givefakeori++; + AddChange(changes.Orientation, value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); + + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + m_buoyancy = value; + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + public override int VehicleType + { + // we may need to put a fake on this + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + AddChange(changes.VehicleType, value); + } + } + + public override void VehicleFloatParam(int param, float value) + { + strVehicleFloatParam fp = new strVehicleFloatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleFloatParam, fp); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + strVehicleVectorParam fp = new strVehicleVectorParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); + } + + public override void VehicleRotationParam(int param, Quaternion value) + { + strVehicleQuatParam fp = new strVehicleQuatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleRotationParam, fp); + } + + public override void VehicleFlags(int param, bool value) + { + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); + } + + public override void SetVehicle(object vdata) + { + AddChange(changes.SetVehicle, vdata); + } + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + if (m_outbounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + m_lastposition = _position; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + if(Body != IntPtr.Zero) + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + if (prim_geom != IntPtr.Zero) + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + + m_outbounds = false; + changeDisable(false); + base.RequestPhysicsterseUpdate(); + } + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + mu = _parent_scene.m_materialContactsData[pMaterial].mu; + bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public void setPrimForRemoval() + { + AddChange(changes.Remove, null); + } + + public override void link(PhysicsActor obj) + { + AddChange(changes.Link, obj); + } + + public override void delink() + { + AddChange(changes.DeLink, null); + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axis); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + _parent_scene.RemoveCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) + CollisionEventsThisFrame = null; + else + CollisionEventsThisFrame = new CollisionEventUpdate(); + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) + { + Name = primName; + LocalID = plocalID; + + m_vehicle = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); + } + _position = pos; + givefakepos = 0; + + PID_D = parent_scene.bodyPIDD; + PID_G = parent_scene.bodyPIDG; + m_density = parent_scene.geomDefaultDensity; + // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; + Body = IntPtr.Zero; + + if (!size.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + } + + _orientation = rotation; + givefakeori = 0; + + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_colliderfilter = 0; + m_softcolide = true; + m_NoColide = false; + + hasOOBoffsetFromMesh = false; + _triMeshData = IntPtr.Zero; + + m_shapetype = _shapeType; + + m_lastdoneSelected = false; + m_isSelected = false; + m_delaySelect = false; + + m_isphantom = pisPhantom; + m_fakeisphantom = pisPhantom; + + mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + CalcPrimBodyData(); + + m_mesh = null; + if (_parent_scene.needsMeshing(pbs)) + { + bool convex; + if (m_shapetype == 0) + convex = false; + else + convex = true; + + m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex); + } + + + m_building = true; // control must set this to false when done + + AddChange(changes.Add, null); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + } + + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); + + if (axisnum <= 0) + return; + + // stop it + d.BodySetTorque(Body, 0, 0, 0); + d.BodySetAngularVel(Body, 0, 0, 0); + + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + + d.JointSetAMotorNumAxes(Amotor, axisnum); + + // get current orientation to lock + + d.Quaternion dcur = d.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; + Vector3 ax; + + int i = 0; + int j = 0; + if (axis.X == 0) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + // ODE should do this with axis relative to body 1 but seems to fail + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); + i++; + j = 256; // move to next axis set + } + + if (axis.Y == 0) + { + ax = (new Vector3(0, 1, 0)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + i++; + j += 256; + } + + if (axis.Z == 0) + { + ax = (new Vector3(0, 0, 1)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + } + } + + private bool setMesh(OdeScene parent_scene) + { + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + if (Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this, false); + } + } + else + { + DestroyBody(); + } + } + + IMesh mesh = null; + + + lock (m_meshlock) + { + if (m_mesh == null) + { + bool convex; + if (m_shapetype == 0) + convex = false; + else + convex = true; + + mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex); + } + else + { + mesh = m_mesh; + } + + if (mesh == null) + { + m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); + return false; + } + + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", + Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); + mesh.releaseSourceMeshData(); + return false; + } + + primOOBoffset = mesh.GetCentroid(); + hasOOBoffsetFromMesh = true; + + mesh.releaseSourceMeshData(); + m_mesh = null; + } + + IntPtr geo = IntPtr.Zero; + + try + { + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + return false; + } + + SetGeom(geo); + return true; + } + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + { + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + CalcPrimBodyData(); + + _parent_scene.geom_name_map[prim_geom] = Name; + _parent_scene.actor_name_map[prim_geom] = this; + + } + else + m_log.Warn("Setting bad Geom"); + } + + + /// + /// Create a geometry for the given mesh in the given target space. + /// + /// + /// If null, then a mesh is used that is based on the profile shape data. + private void CreateGeom() + { + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + + bool haveMesh = false; + hasOOBoffsetFromMesh = false; + m_NoColide = false; + + if (_parent_scene.needsMeshing(_pbs)) + { + haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + if (!haveMesh) + m_NoColide = true; + } + + if (!haveMesh) + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return; + } + } + else + {// do it as a box + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + //Console.WriteLine(" CreateGeom 4"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + } + } + + /// + /// Set a new geometry for this prim. + /// + /// + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(prim_geom); + _parent_scene.actor_name_map.Remove(prim_geom); + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + // catch (System.AccessViolationException) + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); + } + + prim_geom = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + Body = IntPtr.Zero; + hasOOBoffsetFromMesh = false; + CalcPrimBodyData(); + } + + private void ChildSetGeom(OdePrim odePrim) + { + // well.. + DestroyBody(); + MakeBody(); + } + + //sets non physical prim m_targetSpace to right space in spaces grid for static prims + // should only be called for non physical prims unless they are becoming non physical + private void SetInStaticSpace(OdePrim prim) + { + IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); + prim.m_targetSpace = targetSpace; + d.GeomEnable(prim_geom); + } + + public void enableBodySoft() + { + if (!childPrim && !m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prm.prim_geom); + } + } + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prim_geom); + } + d.BodyEnable(Body); + } + } + m_disabled = false; + resetCollisionAccounting(); // this sets m_disable to false + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero) + { + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prm.prim_geom); + } + } + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prim_geom); + } + + d.BodyDisable(Body); + } + } + } + + private void MakeBody() + { + if (!m_isphysical) // only physical get bodies + return; + + if (childPrim) // child prims don't get bodies; + return; + + if (m_building) + return; + + if (prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); + return; + } + + if (Body != IntPtr.Zero) + { + d.BodyDestroy(Body); + Body = IntPtr.Zero; + m_log.Warn("[PHYSICS]: MakeBody called having a body"); + } + + + if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); + } + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + Body = d.BodyCreate(_parent_scene.world); + + DMassDup(ref primdMass, out objdmass); + + // rotate inertia + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + // set the body rotation + d.BodySetRotation(Body, ref mymat); + + // recompute full object inertia if needed + if (childrenPrim.Count > 0) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; + Vector3 rcm; + + rcm.X = _position.X + objdmass.c.X; + rcm.Y = _position.Y + objdmass.c.Y; + rcm.Z = _position.Z + objdmass.c.Z; + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); + continue; + } + + DMassCopy(ref prm.primdMass, ref tmpdmass); + + // apply prim current rotation to inertia + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + quat.W = prm._orientation.W; + d.RfromQ(out mat, ref quat); + d.MassRotate(ref tmpdmass, ref mat); + + Vector3 ppos = prm._position; + ppos.X += tmpdmass.c.X - rcm.X; + ppos.Y += tmpdmass.c.Y - rcm.Y; + ppos.Z += tmpdmass.c.Z - rcm.Z; + + // refer inertia to root prim center of mass position + d.MassTranslate(ref tmpdmass, + ppos.X, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats + + if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); + } + + d.GeomClearOffset(prm.prim_geom); + d.GeomSetBody(prm.prim_geom, Body); + prm.Body = Body; + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.W = -myrot.W; + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + // d.BodySetLinearDampingThreshold(Body, 0.01f); + // d.BodySetAngularDampingThreshold(Body, 0.001f); + d.BodySetDamping(Body, .002f, .002f); + + + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } + + + if (childrenPrim.Count == 0) + { + collide_geom = prim_geom; + m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prim_geom); + } + else + { + m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); + d.HashSpaceSetLevels(m_targetSpace, -2, 8); + d.SpaceSetSublevel(m_targetSpace, 3); + d.SpaceSetCleanup(m_targetSpace, false); + d.SpaceAdd(m_targetSpace, prim_geom); + collide_geom = m_targetSpace; + } + + if (m_delaySelect) + { + m_isSelected = true; + m_delaySelect = false; + } + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + continue; + + Vector3 ppos = prm._position; + d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position + + if (prm.m_targetSpace != m_targetSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = m_targetSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } + + if (m_isSelected || m_disabled) + { + prm.m_collisionCategories &= ~CollisionCategories.Body; + prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); + d.GeomDisable(prm.prim_geom); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = CollisionCategories.Land; + } + else + { + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + d.GeomEnable(prm.prim_geom); + } + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + d.GeomEnable(prm.prim_geom); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + } + prm.m_collisionscore = 0; + + if(!m_disabled) + prm.m_disabled = false; + + _parent_scene.addActivePrim(prm); + } + } + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + + if (m_isSelected || m_disabled) + { + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); + + d.GeomDisable(prim_geom); + d.BodyDisable(Body); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + m_collisionFlags = CollisionCategories.Land; + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } + + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + m_collisionscore = 0; + + m_softcolide = true; + _parent_scene.addActivePrim(this); + _parent_scene.addActiveGroups(this); + } + + private void DestroyBody() + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + + if (!childPrim) + { + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); + } + prm.Body = IntPtr.Zero; + prm._mass = prm.primMass; + prm.m_collisionscore = 0; + } + } + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + _parent_scene.remActiveGroup(this); + d.BodyDestroy(Body); + } + Body = IntPtr.Zero; + } + _mass = primMass; + m_collisionscore = 0; + } + + private void FixInertia(Vector3 NewPos,Quaternion newrot) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + + d.BodyGetMass(Body, out tmpdmass); + objdmass = tmpdmass; + + d.Vector3 dobjpos; + d.Vector3 thispos; + + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + // get prim own inertia in its local frame + tmpdmass = primdMass; + + // transform to object frame + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // back prim own inertia + tmpdmass = primdMass; + + // update to new position and orientation + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } + + + + private void FixInertia(Vector3 NewPos) + { + d.Matrix3 primmat = new d.Matrix3(); + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Mass primmass = new d.Mass { }; + + d.Vector3 dobjpos; + d.Vector3 thispos; + + d.BodyGetMass(Body, out objdmass); + + // get prim own inertia in its local frame + primmass = primdMass; + // transform to object frame + primmat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref primmass, ref primmat); + + tmpdmass = primmass; + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // update to new position + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref primmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref primmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } + + private void FixInertia(Quaternion newrot) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Vector3 dobjpos; + d.Vector3 thispos; + + d.BodyGetMass(Body, out objdmass); + + // get prim own inertia in its local frame + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + // transform to object frame + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // update to new orientation + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } + + + #region Mass Calculation + + private float CalculatePrimVolume() + { + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.52359877559829887307710723054658f; + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + return volume; + } + + + private void CalcPrimBodyData() + { + float volume; + + if (prim_geom == IntPtr.Zero) + { + // Ubit let's have a initial basic OOB + primOOBsize.X = _size.X; + primOOBsize.Y = _size.Y; + primOOBsize.Z = _size.Z; + primOOBoffset = Vector3.Zero; + } + else + { + d.AABB AABB; + d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom + + primOOBsize.X = (AABB.MaxX - AABB.MinX); + primOOBsize.Y = (AABB.MaxY - AABB.MinY); + primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); + if (!hasOOBoffsetFromMesh) + { + primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; + primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; + primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; + } + } + + // also its own inertia and mass + // keep using basic shape mass for now + volume = CalculatePrimVolume(); + + primMass = m_density * volume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + _mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); + + d.MassTranslate(ref primdMass, + primOOBoffset.X, + primOOBoffset.Y, + primOOBoffset.Z); + + primOOBsize *= 0.5f; // let obb size be a corner coords + primOOBradiusSQ = primOOBsize.LengthSquared(); + } + + + #endregion + + + /// + /// Add a child prim to this parent prim. + /// + /// Child prim + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) + { + //Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) + { + DestroyBody(); // for now we need to rebuil entire object on link change + + lock (childrenPrim) + { + // adopt the prim + if (!childrenPrim.Contains(prim)) + childrenPrim.Add(prim); + + // see if this prim has kids and adopt them also + // should not happen for now + foreach (OdePrim prm in prim.childrenPrim) + { + if (!childrenPrim.Contains(prm)) + { + if (prm.Body != IntPtr.Zero) + { + if (prm.prim_geom != IntPtr.Zero) + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + if (prm.Body != prim.Body) + prm.DestroyBody(); // don't loose bodies around + prm.Body = IntPtr.Zero; + } + + childrenPrim.Add(prm); + prm._parent = this; + } + } + } + //Remove old children from the prim + prim.childrenPrim.Clear(); + + if (prim.Body != IntPtr.Zero) + { + if (prim.prim_geom != IntPtr.Zero) + d.GeomSetBody(prim.prim_geom, IntPtr.Zero); + prim.DestroyBody(); // don't loose bodies around + prim.Body = IntPtr.Zero; + } + + prim.childPrim = true; + prim._parent = this; + + MakeBody(); // full nasty reconstruction + } + } + + private void UpdateChildsfromgeom() + { + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.UpdateDataFromGeom(); + } + } + + private void UpdateDataFromGeom() + { + if (prim_geom != IntPtr.Zero) + { + d.Quaternion qtmp = new d.Quaternion { }; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.W = qtmp.W; + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + } + } + + private void ChildDelink(OdePrim odePrim, bool remakebodies) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) // delinking the root prim + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + if (remakebodies) + newroot.MakeBody(); + } + } + } + + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + // odePrim.UpdateDataFromGeom(); + if (remakebodies) + odePrim.MakeBody(); + } + } + if (remakebodies) + MakeBody(); + } + + protected void ChildRemove(OdePrim odePrim, bool reMakeBody) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + newroot.MakeBody(); + } + } + if (reMakeBody) + MakeBody(); + return; + } + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + if (reMakeBody) + odePrim.MakeBody(); + } + } + MakeBody(); + } + + #region changes + + private void changeadd() + { + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + + if (!m_isphysical) + SetInStaticSpace(this); + } + + if (m_isphysical && Body == IntPtr.Zero) + { + MakeBody(); + } + } + + private void changeAngularLock(Vector3 newLock) + { + // do we have a Physical object? + if (Body != IntPtr.Zero) + { + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) + { + if (!newLock.ApproxEquals(Vector3.One, 0f)) + { + createAMotor(newLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = newLock; + } + + private void changeLink(OdePrim NewParent) + { + if (_parent == null && NewParent != null) + { + NewParent.ParentPrim(this); + } + else if (_parent != null) + { + if (_parent is OdePrim) + { + if (NewParent != _parent) + { + (_parent as OdePrim).ChildDelink(this, false); // for now... + childPrim = false; + + if (NewParent != null) + { + NewParent.ParentPrim(this); + } + } + } + } + _parent = NewParent; + } + + + private void Stop() + { + if (!childPrim) + { + m_force = Vector3.Zero; + m_forceacc = Vector3.Zero; + m_angularForceacc = Vector3.Zero; + _torque = Vector3.Zero; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + } + + if (Body != IntPtr.Zero) + { + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetTorque(Body, 0f, 0f, 0f); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetAngularVel(Body, 0f, 0f, 0f); + } + } + + + private void changePhantomStatus(bool newval) + { + m_isphantom = newval; + + if (m_isSelected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + } + else + { + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; // should never happen + } + + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } + } + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + if(!m_isSelected) + d.GeomEnable(prm.prim_geom); + } + } + } + + if (!m_disabled && prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + if(!m_isSelected) + d.GeomEnable(prim_geom); + } + } + + private void changeSelectedStatus(bool newval) + { + if (m_lastdoneSelected == newval) + return; + + m_lastdoneSelected = newval; + DoSelectedStatus(newval); + } + + private void CheckDelaySelect() + { + if (m_delaySelect) + { + DoSelectedStatus(m_isSelected); + } + } + + private void DoSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); + + if (newval) + { + if (!childPrim && Body != IntPtr.Zero) + d.BodyDisable(Body); + + if (m_delaySelect || m_isphysical) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != null) + { + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prm.prim_geom); + } + prm.m_delaySelect = false; + } + } + + if (prim_geom != null) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomDisable(prim_geom); + } + + m_delaySelect = false; + } + else if(!m_isphysical) + { + m_delaySelect = true; + } + } + else + { + if (!childPrim && Body != IntPtr.Zero && !m_disabled) + d.BodyEnable(Body); + + if (m_isphantom && !m_isVolumeDetect) + { + m_collisionCategories = 0; + if(m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; + + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + } + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prm.prim_geom); + } + prm.m_delaySelect = false; + prm.m_softcolide = true; + } + } + + if (!m_disabled && prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + d.GeomEnable(prim_geom); + } + + m_delaySelect = false; + m_softcolide = true; + } + + resetCollisionAccounting(); + } + + private void changePosition(Vector3 newPos) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _position = newPos; + } + + else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) + { + FixInertia(newPos); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + // changeSelectedStatus(); + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changeOrientation(Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _orientation = newOri; + } + /* + else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) + { + FixInertia(_position, newOri); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + */ + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + } + } + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim && m_building) // inertia is messed, must rebuild + { + _position = newPos; + _orientation = newOri; + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + + m_softcolide = true; + resetCollisionAccounting(); + } + + + private void changeDisable(bool disable) + { + if (disable) + { + if (!m_disabled) + disableBodySoft(); + } + else + { + if (m_disabled) + enableBodySoft(); + } + } + + private void changePhysicsStatus(bool NewStatus) + { + CheckDelaySelect(); + + m_isphysical = NewStatus; + + if (!childPrim) + { + if (NewStatus) + { + if (Body == IntPtr.Zero) + MakeBody(); + } + else + { + if (Body != IntPtr.Zero) + { + DestroyBody(); + } + Stop(); + } + } + + resetCollisionAccounting(); + } + + private void changeprimsizeshape() + { + CheckDelaySelect(); + + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + // we don't need to do space calculation because the client sends a position update also. + if (_size.X <= 0) + _size.X = 0.01f; + if (_size.Y <= 0) + _size.Y = 0.01f; + if (_size.Z <= 0) + _size.Z = 0.01f; + // Construction of new prim + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + + m_softcolide = true; + resetCollisionAccounting(); + } + + private void changeSize(Vector3 newSize) + { + _size = newSize; + changeprimsizeshape(); + } + + private void changeShape(PrimitiveBaseShape newShape) + { + if(newShape != null) + _pbs = newShape; + changeprimsizeshape(); + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; + + if (prim_geom != IntPtr.Zero && !m_isphantom) + { + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } + else + { + m_collisionFlags &= ~CollisionCategories.Water; + } + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } + + private void changeSetTorque(Vector3 newtorque) + { + if (!m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + } + _torque = newtorque; + } + } + + private void changeForce(Vector3 force) + { + m_force = force; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeAddForce(Vector3 force) + { + m_forceacc += force; + if (!m_isSelected) + { + lock (this) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + + m_collisionscore = 0; + } + } + + private void changeAddAngularForce(Vector3 aforce) + { + m_angularForceacc += aforce; + if (!m_isSelected) + { + lock (this) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + m_collisionscore = 0; + } + } + + private void changevelocity(Vector3 newVel) + { + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; + } + + private void changeVolumedetetion(bool newVolDtc) + { + m_isVolumeDetect = newVolDtc; + } + + protected void changeBuilding(bool newbuilding) + { + if ((bool)newbuilding) + { + m_building = true; + if (!childPrim) + DestroyBody(); + } + else + { + m_building = false; + CheckDelaySelect(); + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } + } + + public void changeSetVehicle(VehicleData vdata) + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + m_vehicle.DoSetVehicle(vdata); + } + private void changeVehicleType(int value) + { + if (value == (int)Vehicle.TYPE_NONE) + { + if (m_vehicle != null) + m_vehicle = null; + } + else + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + + private void changeVehicleFloatParam(strVehicleFloatParam fp) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + } + + private void changeVehicleVectorParam(strVehicleVectorParam vp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + } + + private void changeVehicleRotationParam(strVehicleQuatParam qp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); + } + + private void changeVehicleFlags(strVehicleBoolParam bp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + } + + #endregion + + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) + // !m_disabled && !m_isSelected && !m_building && !m_outbounds) + { +// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + float timestep = _parent_scene.ODE_STEPSIZE; + + // check outside region + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; + + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if(m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } + + + float fx = 0; + float fy = 0; + float fz = 0; + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + } + else + { + float m_mass = _mass; + + // fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + if (m_usePID) + { + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID) + { + //Console.WriteLine("Hover " + Name); + + // If we're using the PID controller, then we have no gravity + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } + else + { + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } + + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; + + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; + + m_forceacc = Vector3.Zero; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + + Vector3 trq; + + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } + + } + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; + //Console.WriteLine("Nothing " + Name); + + } + } + + + public void UpdatePositionAndVelocity(float simulatedtime) + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null && !m_disabled && !m_building && !m_outbounds) + { + if (Body != IntPtr.Zero) + { + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + + if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) + && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) + && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) + && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) + && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) + && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) + ) + { + _zeroFlag = true; + //Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } + + if (_zeroFlag) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastVelocity = _velocity; + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + + _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + { + m_rotationalVelocity = pv; + } + else + { + m_rotationalVelocity.X = rotvel.X; + m_rotationalVelocity.Y = rotvel.Y; + m_rotationalVelocity.Z = rotvel.Z; + } + + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + m_lastposition = _position; + m_lastorientation = _orientation; + base.RequestPhysicsterseUpdate(); + } + else + { + throttleCounter++; + } + } + } + else if (!m_lastUpdateSent || !_zeroFlag) + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + } + } + + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } + + internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) + { + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + + internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) + { + // assumes object center of mass is zero + float smass = part.mass; + theobj.mass -= smass; + + smass *= 1.0f / (theobj.mass); ; + + theobj.c.X -= part.c.X * smass; + theobj.c.Y -= part.c.Y * smass; + theobj.c.Z -= part.c.Z * smass; + + theobj.I.M00 -= part.I.M00; + theobj.I.M01 -= part.I.M01; + theobj.I.M02 -= part.I.M02; + theobj.I.M10 -= part.I.M10; + theobj.I.M11 -= part.I.M11; + theobj.I.M12 -= part.I.M12; + theobj.I.M20 -= part.I.M20; + theobj.I.M21 -= part.I.M21; + theobj.I.M22 -= part.I.M22; + } + + private static void DMassDup(ref d.Mass src, out d.Mass dst) + { + dst = new d.Mass { }; + + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; + case changes.Remove: + //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... + //When we return true, it destroys all of the prims in the linkset anyway + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildRemove(this, false); + } + else + ChildRemove(this, false); + + m_vehicle = null; + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + if (m_eventsubscription > 0) + UnSubscribeEvents(); + return true; + + case changes.Link: + OdePrim tmp = (OdePrim)arg; + changeLink(tmp); + break; + + case changes.DeLink: + changeLink(null); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changevelocity((Vector3)arg); + break; + + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Phantom: + changePhantomStatus((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; + + case changes.VehicleType: + changeVehicleType((int)arg); + break; + + case changes.VehicleFlags: + changeVehicleFlags((strVehicleBoolParam) arg); + break; + + case changes.VehicleFloatParam: + changeVehicleFloatParam((strVehicleFloatParam) arg); + break; + + case changes.VehicleVectorParam: + changeVehicleVectorParam((strVehicleVectorParam) arg); + break; + + case changes.VehicleRotationParam: + changeVehicleRotationParam((strVehicleQuatParam) arg); + break; + + case changes.SetVehicle: + changeSetVehicle((VehicleData) arg); + break; + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor) this, what, arg); + } + + + private struct strVehicleBoolParam + { + public int param; + public bool value; + } + + private struct strVehicleFloatParam + { + public int param; + public float value; + } + + private struct strVehicleQuatParam + { + public int param; + public Quaternion value; + } + + private struct strVehicleVectorParam + { + public int param; + public Vector3 value; + } + } } \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 3fc2de3..1a6907d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -160,8 +160,8 @@ namespace OpenSim.Region.Physics.OdePlugin private Random fluidRandomizer = new Random(Environment.TickCount); const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; - const float comumContactERP = 0.6f; - const float comumSoftContactERP = 0.1f; + const float MaxERP = 0.8f; + const float minERP = 0.1f; const float comumContactCFM = 0.0001f; float frictionMovementMult = 0.3f; @@ -527,7 +527,7 @@ namespace OpenSim.Region.Physics.OdePlugin // sets a global contact for a joint for contactgeom , and base contact description) - private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, bool softerp) + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce,float cfm,float erp) { if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) return IntPtr.Zero; @@ -545,11 +545,8 @@ namespace OpenSim.Region.Physics.OdePlugin newcontact.surface.mode = comumContactFlags; newcontact.surface.mu = mu; newcontact.surface.bounce = bounce; - newcontact.surface.soft_cfm = comumContactCFM; - if (softerp) - newcontact.surface.soft_erp = comumSoftContactERP; - else - newcontact.surface.soft_erp = comumContactERP; + newcontact.surface.soft_cfm = cfm; + newcontact.surface.soft_erp = erp; IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); Marshal.StructureToPtr(newcontact, contact, true); @@ -693,9 +690,11 @@ namespace OpenSim.Region.Physics.OdePlugin // big messy collision analises float mu = 0; float bounce = 0; + float cfm = 0.0001f; + float erp = 0.1f; + ContactData contactdata1 = new ContactData(0, 0, false); ContactData contactdata2 = new ContactData(0, 0, false); - bool erpSoft = false; String name = null; bool dop1foot = false; @@ -705,61 +704,65 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p1.PhysicsActorType) { case (int)ActorTypes.Agent: - switch (p2.PhysicsActorType) { - case (int)ActorTypes.Agent: - p1.getContactData(ref contactdata1); - p2.getContactData(ref contactdata2); + bounce = 0; + mu = 0; + cfm = 0.0001f; - bounce = 0; - - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: +/* + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); - if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) - mu *= frictionMovementMult; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - erpSoft = contactdata1.softcolide | contactdata2.softcolide; - p1.CollidingObj = true; - p2.CollidingObj = true; - break; - case (int)ActorTypes.Prim: - p1.getContactData(ref contactdata1); - p2.getContactData(ref contactdata2); + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; +*/ + p1.CollidingObj = true; + p2.CollidingObj = true; + break; + case (int)ActorTypes.Prim: +/* + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); - bounce = 0; - - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) - mu *= frictionMovementMult; - if (p2.Velocity.LengthSquared() > 0.0f) - p2.CollidingObj = true; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - erpSoft = contactdata1.softcolide | contactdata2.softcolide; + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + */ + if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; - dop1foot = true; - break; - default: - ignore=true; // avatar to terrain and water ignored - break; + dop1foot = true; + break; + default: + ignore = true; // avatar to terrain and water ignored + break; + } + break; } - break; case (int)ActorTypes.Prim: switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: - p1.getContactData(ref contactdata1); - p2.getContactData(ref contactdata2); +// p1.getContactData(ref contactdata1); +// p2.getContactData(ref contactdata2); bounce = 0; - + mu = 0; + cfm = 0.0001f; +/* mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; - - erpSoft = contactdata1.softcolide | contactdata2.softcolide; +*/ dop2foot = true; if (p1.Velocity.LengthSquared() > 0.0f) p1.CollidingObj = true; @@ -773,9 +776,16 @@ namespace OpenSim.Region.Physics.OdePlugin p1.getContactData(ref contactdata1); p2.getContactData(ref contactdata2); bounce = contactdata1.bounce * contactdata2.bounce; - erpSoft = contactdata1.softcolide | contactdata2.softcolide; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + cfm = p1.Mass; + if (cfm > p2.Mass) + cfm = p2.Mass; + cfm = (float)Math.Sqrt(cfm); + cfm *= 0.0001f; + if (cfm > 0.8f) + cfm = 0.8f; + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; @@ -790,12 +800,17 @@ namespace OpenSim.Region.Physics.OdePlugin mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) mu *= frictionMovementMult; - erpSoft = contactdata1.softcolide; p1.CollidingGround = true; + cfm = p1.Mass; + cfm = (float)Math.Sqrt(cfm); + cfm *= 0.0001f; + if (cfm > 0.8f) + cfm = 0.8f; + } else if (name == "Water") { - erpSoft = true; + ignore = true; } } else @@ -815,7 +830,11 @@ namespace OpenSim.Region.Physics.OdePlugin p2.getContactData(ref contactdata2); bounce = contactdata2.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); - erpSoft = contactdata2.softcolide; + cfm = p2.Mass; + cfm = (float)Math.Sqrt(cfm); + cfm *= 0.0001f; + if (cfm > 0.8f) + cfm = 0.8f; if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) mu *= frictionMovementMult; @@ -827,7 +846,7 @@ namespace OpenSim.Region.Physics.OdePlugin else if (name == "Water" && (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent)) { - erpSoft = true; + ignore = true; } } else @@ -848,7 +867,14 @@ namespace OpenSim.Region.Physics.OdePlugin if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) p2.IsColliding = true; - Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft); + + erp = curContact.depth; + if (erp < minERP) + erp = minERP; + else if (erp > MaxERP) + erp = MaxERP; + + Joint = CreateContacJoint(ref curContact, mu, bounce,cfm,erp); d.JointAttach(Joint, b1, b2); if (++m_global_contactcount >= maxContactsbeforedeath) -- cgit v1.1 From c4e4e04eee0741795e7b9fe53cc5820422755683 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 31 Mar 2012 00:03:45 +0100 Subject: reduced instability in vertical atractor with eficiency of 1 and banking this will need a nicer solution sometime... --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index d0b4546..dcd02e2 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -153,7 +153,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_linearFrictionTimescale.Z < timestep) m_linearFrictionTimescale.Z = timestep; m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; - if (m_linearMotorDecayTimescale < 0.5f) m_linearMotorDecayTimescale = 0.5f; + if (m_linearMotorDecayTimescale < timestep) m_linearMotorDecayTimescale = timestep; m_linearMotorDecayTimescale *= invtimestep; m_linearMotorTimescale = vd.m_linearMotorTimescale; @@ -168,7 +168,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_angularMotorTimescale < timestep) m_angularMotorTimescale = timestep; m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; - if (m_angularMotorDecayTimescale < 0.5f) m_angularMotorDecayTimescale = 0.5f; + if (m_angularMotorDecayTimescale < timestep) m_angularMotorDecayTimescale = timestep; m_angularMotorDecayTimescale *= invtimestep; m_angularFrictionTimescale = vd.m_angularFrictionTimescale; @@ -230,9 +230,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularDeflectionTimescale = pValue; break; case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - // if (pValue < timestep) pValue = timestep; + if (pValue < timestep) pValue = timestep; // try to make impulses to work a bit better - if (pValue < 0.5f) pValue = 0.5f; +// if (pValue < 0.5f) pValue = 0.5f; else if (pValue > 120) pValue = 120; m_angularMotorDecayTimescale = pValue * invtimestep; break; @@ -281,9 +281,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearDeflectionTimescale = pValue; break; case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - // if (pValue < timestep) pValue = timestep; + if (pValue < timestep) pValue = timestep; // try to make impulses to work a bit better - if (pValue < 0.5f) pValue = 0.5f; + //if (pValue < 0.5f) pValue = 0.5f; else if (pValue > 120) pValue = 120; m_linearMotorDecayTimescale = pValue * invtimestep; break; @@ -444,9 +444,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120; + m_linearMotorDecayTimescale = 120 * invtimestep; m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 1000; + m_angularMotorDecayTimescale = 1000 * invtimestep; m_VhoverHeight = 0; m_VhoverEfficiency = 1; m_VhoverTimescale = 1000; @@ -901,7 +901,11 @@ namespace OpenSim.Region.Physics.OdePlugin GetRollPitch(irotq, out roll, out pitch); float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; - float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; + float ftmp2; + if (m_bankingEfficiency == 0) + ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; + else + ftmp2 = 0; if (roll > halfpi) roll = pi - roll; -- cgit v1.1 From 39079a62c038986b20ccc3cf89a2e169f6e6135f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 9 Apr 2012 20:58:40 +0100 Subject: chODE and ubitODE always return prim mass (they where returning object mass on physical prims) so SOG can do the total add. (ubitODE as more code to use a simpler terrain geom on a modified ode lib but should do autodetect and work with normal lib). --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 11 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 53 +++- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 318 ++++++++++++++++++----- 4 files changed, 315 insertions(+), 69 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 3e2b71c..77ea2af 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -179,6 +179,8 @@ namespace OpenSim.Region.Physics.OdePlugin public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; + private float m_primMass = 10.000006836f; // Aluminum g/cm3; + private byte m_shapetype; private byte m_taintshapetype; @@ -538,7 +540,11 @@ namespace OpenSim.Region.Physics.OdePlugin public override float Mass { - get { return CalculateMass(); } + get + { + CalculateMass(); + return m_primMass; + } } public override Vector3 Force @@ -1316,6 +1322,9 @@ namespace OpenSim.Region.Physics.OdePlugin + m_primMass = returnMass; + if (m_primMass > _parent_scene.maximumMassObject) + m_primMass = _parent_scene.maximumMassObject; // Recursively calculate mass bool HasChildPrim = false; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index c4dc793..f739183 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -395,7 +395,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override float Mass { - get { return _mass; } + get { return primMass; } } public override Vector3 Force diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 2b6bc59..f5129cb 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -107,16 +107,17 @@ namespace OdeAPI ConvexClass, GeomTransformClass, TriMeshClass, - HeightfieldClass, + HeightfieldClass, FirstSpaceClass, SimpleSpaceClass = FirstSpaceClass, HashSpaceClass, QuadTreeSpaceClass, LastSpaceClass = QuadTreeSpaceClass, + UbitTerrainClass, FirstUserClass, LastUserClass = FirstUserClass + MaxUserClasses - 1, NumClasses, - MaxUserClasses = 4 + MaxUserClasses = 5 } public enum JointType : int @@ -201,8 +202,11 @@ namespace OdeAPI [UnmanagedFunctionPointer(CallingConvention.Cdecl)] public delegate void GeomDtorFn(IntPtr o); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal UbitTerrainGetHeight(IntPtr p_user_data, int x, int z); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); @@ -729,6 +733,18 @@ namespace OdeAPI return CreateiHeightfield(space, data, bPlaceable); } + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateUbitTerrain"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiUbitTerrain(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateUbitTerrain(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiUbitTerrain(space, data, bPlaceable); + } + + + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] public static extern IntPtr CreateiGeom(int classnum); public static IntPtr CreateGeom(int classnum) @@ -964,6 +980,8 @@ namespace OdeAPI dReal width, dReal depth, int widthSamples, int depthSamples, dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, dReal width, dReal depth, int widthSamples, int depthSamples, @@ -989,6 +1007,33 @@ namespace OdeAPI [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomUbitTerrainDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomUbitTerrainGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomUbitTerrainSetHeightfieldData(IntPtr g, IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] public static extern bool GeomIsEnabled(IntPtr geom); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 1a6907d..3e0ccef 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -156,6 +156,7 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly ILog m_log; // private Dictionary m_storedCollisions = new Dictionary(); + public bool OdeUbitLib = false; // private int threadid = 0; private Random fluidRandomizer = new Random(Environment.TickCount); @@ -374,7 +375,14 @@ namespace OpenSim.Region.Physics.OdePlugin mesher = meshmerizer; m_config = config; -// m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE")); + string ode_config = d.GetConfiguration("ODE"); + m_log.WarnFormat("ODE configuration: {0}", ode_config); + + if (ode_config.Contains("ODE_Ubit")) + { + OdeUbitLib = true; + } + /* if (region != null) { @@ -527,13 +535,24 @@ namespace OpenSim.Region.Physics.OdePlugin // sets a global contact for a joint for contactgeom , and base contact description) - private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce,float cfm,float erp) + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale) { if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) return IntPtr.Zero; + float erp = contactGeom.depth; + erp *= erpscale; + if (erp < minERP) + erp = minERP; + else if (erp > MaxERP) + erp = MaxERP; + + float depth = contactGeom.depth * dscale; + if (depth > 0.5f) + depth = 0.5f; + d.Contact newcontact = new d.Contact(); - newcontact.geom.depth = contactGeom.depth; + newcontact.geom.depth = depth; newcontact.geom.g1 = contactGeom.g1; newcontact.geom.g2 = contactGeom.g2; newcontact.geom.pos = contactGeom.pos; @@ -692,6 +711,10 @@ namespace OpenSim.Region.Physics.OdePlugin float bounce = 0; float cfm = 0.0001f; float erp = 0.1f; + float erpscale = 1.0f; + float dscale = 1.0f; + bool IgnoreNegSides = false; + ContactData contactdata1 = new ContactData(0, 0, false); ContactData contactdata2 = new ContactData(0, 0, false); @@ -781,10 +804,14 @@ namespace OpenSim.Region.Physics.OdePlugin cfm = p1.Mass; if (cfm > p2.Mass) cfm = p2.Mass; - cfm = (float)Math.Sqrt(cfm); - cfm *= 0.0001f; - if (cfm > 0.8f) - cfm = 0.8f; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + if (dscale > 1.0f) + dscale = 1.0f; + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; @@ -801,11 +828,22 @@ namespace OpenSim.Region.Physics.OdePlugin if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) mu *= frictionMovementMult; p1.CollidingGround = true; + cfm = p1.Mass; - cfm = (float)Math.Sqrt(cfm); - cfm *= 0.0001f; - if (cfm > 0.8f) - cfm = 0.8f; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + if (dscale > 1.0f) + dscale = 1.0f; + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + + if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) + { + if (curContact.side1 > 0) + IgnoreNegSides = true; + } } else if (name == "Water") @@ -830,11 +868,21 @@ namespace OpenSim.Region.Physics.OdePlugin p2.getContactData(ref contactdata2); bounce = contactdata2.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + cfm = p2.Mass; - cfm = (float)Math.Sqrt(cfm); - cfm *= 0.0001f; - if (cfm > 0.8f) - cfm = 0.8f; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + + if (dscale > 1.0f) + dscale = 1.0f; + + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + + if (curContact.side1 > 0) // should be 2 ? + IgnoreNegSides = true; if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) mu *= frictionMovementMult; @@ -862,39 +910,45 @@ namespace OpenSim.Region.Physics.OdePlugin int i = 0; while(true) { - if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) - p1.IsColliding = true; - if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) - p2.IsColliding = true; + if (IgnoreNegSides && curContact.side1 < 0) + { + if (++i >= count) + break; - erp = curContact.depth; - if (erp < minERP) - erp = minERP; - else if (erp > MaxERP) - erp = MaxERP; + if (!GetCurContactGeom(i, ref curContact)) + break; + } + else - Joint = CreateContacJoint(ref curContact, mu, bounce,cfm,erp); - d.JointAttach(Joint, b1, b2); + { + if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) + p1.IsColliding = true; + if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) + p2.IsColliding = true; - if (++m_global_contactcount >= maxContactsbeforedeath) - break; + Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); + d.JointAttach(Joint, b1, b2); - if(++i >= count) - break; + if (++m_global_contactcount >= maxContactsbeforedeath) + break; - if (!GetCurContactGeom(i, ref curContact)) - break; + if (++i >= count) + break; - if (curContact.depth > maxDepthContact.PenetrationDepth) - { - maxDepthContact.Position.X = curContact.pos.X; - maxDepthContact.Position.Y = curContact.pos.Y; - maxDepthContact.Position.Z = curContact.pos.Z; - maxDepthContact.SurfaceNormal.X = curContact.normal.X; - maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; - maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; - maxDepthContact.PenetrationDepth = curContact.depth; + if (!GetCurContactGeom(i, ref curContact)) + break; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.SurfaceNormal.X = curContact.normal.X; + maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; + maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; + maxDepthContact.PenetrationDepth = curContact.depth; + } } } @@ -1865,13 +1919,12 @@ namespace OpenSim.Region.Physics.OdePlugin public float GetTerrainHeightAtXY(float x, float y) { - // assumes 1m size grid and constante size square regions - // needs to know about sims around in future - // region offset in mega position + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + IntPtr heightFieldGeom = IntPtr.Zero; // get region map @@ -1903,28 +1956,55 @@ namespace OpenSim.Region.Physics.OdePlugin int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples - // we still have square fixed size regions - // also flip x and y because of how map is done for ODE fliped axis - // so ix,iy,dx and dy are inter exchanged - if (x < regsize - 1) - { - iy = (int)x; - dy = x - (float)iy; - } - else // out world use external height + if (OdeUbitLib) { - iy = regsize - 1; - dy = 0; - } - if (y < regsize - 1) - { - ix = (int)y; - dx = y - (float)ix; + if (x < regsize - 1) + { + ix = (int)x; + dx = x - (float)ix; + } + else // out world use external height + { + ix = regsize - 1; + dx = 0; + } + if (y < regsize - 1) + { + iy = (int)y; + dy = y - (float)iy; + } + else + { + iy = regsize - 1; + dy = 0; + } } + else { - ix = regsize - 1; - dx = 0; + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsize - 1; + dy = 0; + } + if (y < regsize - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsize - 1; + dx = 0; + } } float h0; @@ -1951,6 +2031,8 @@ namespace OpenSim.Region.Physics.OdePlugin return h0 + h1 + h2; } + + public override void SetTerrain(float[] heightMap) { if (m_worldOffset != Vector3.Zero && m_parentScene != null) @@ -1972,7 +2054,15 @@ namespace OpenSim.Region.Physics.OdePlugin } public void SetTerrain(float[] heightMap, Vector3 pOffset) - { + { + if (OdeUbitLib) + UbitSetTerrain(heightMap, pOffset); + else + OriSetTerrain(heightMap, pOffset); + } + + public void OriSetTerrain(float[] heightMap, Vector3 pOffset) + { // assumes 1m size grid and constante size square regions // needs to know about sims around in future @@ -2086,6 +2176,108 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public void UbitSetTerrain(float[] heightMap, Vector3 pOffset) + { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future + + float[] _heightmap; + + uint heightmapWidth = Constants.RegionSize + 2; + uint heightmapHeight = Constants.RegionSize + 2; + + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; + + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; + + + uint regionsize = Constants.RegionSize; + + float hfmin = float.MaxValue; +// float hfmax = float.MinValue; + float val; + + + uint maxXXYY = regionsize - 1; + // adding one margin all around so things don't fall in edges + + uint xx; + uint yy = 0; + uint yt = 0; + + for (uint y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxXXYY) + yy += regionsize; + xx = 0; + for (uint x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && x < maxXXYY) + xx++; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode + _heightmap[yt + x] = val; + + if (hfmin > val) + hfmin = val; +// if (hfmax < val) +// hfmax = val; + } + yt += heightmapWidthSamples; + } + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + TerrainHeightFieldHeights.Remove(GroundGeom); + } + d.SpaceRemove(StaticSpace, GroundGeom); + d.GeomDestroy(GroundGeom); + } + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + + const int wrap = 0; + float thickness = hfmin; + if (thickness < 0) + thickness = 1; + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, + thickness, wrap); + +// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[GroundGeom] = "Terrain"; + + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); + RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); + // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + } + } + + public override void DeleteTerrain() { } -- cgit v1.1 From 3999822e13d7ae2f6ab1c19a19a01e0cc7c7acd7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 14 Apr 2012 05:07:52 +0100 Subject: Use chode character actor.SetMomentum() to force full restore Velocity in scenepresence TeleportWithMomentum(), since actor.Velocity was selected by original coders as the input of a desired velocity (even 'forces') that is modified by character conditions, like not changing velocity.Z if it is in free fall. --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 2945199..1f1ba95 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -140,6 +140,10 @@ namespace OpenSim.Region.Physics.OdePlugin public int m_eventsubscription = 0; private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + private Vector3 m_taintMomentum = Vector3.Zero; + private bool m_haveTaintMomentum = false; + + // unique UUID of this character object public UUID m_uuid; public bool bad = false; @@ -800,8 +804,8 @@ namespace OpenSim.Region.Physics.OdePlugin { if (value.IsFinite()) { - m_pidControllerActive = true; _target_velocity = value; + m_pidControllerActive = true; } else { @@ -911,6 +915,12 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SetMomentum(Vector3 momentum) { + if (momentum.IsFinite()) + { + m_taintMomentum = momentum; + m_haveTaintMomentum = true; + _parent_scene.AddPhysicsActorTaint(this); + } } @@ -1424,6 +1434,14 @@ namespace OpenSim.Region.Physics.OdePlugin } } + if (m_haveTaintMomentum) + { + m_haveTaintMomentum = false; + _velocity = m_taintMomentum; + _target_velocity = m_taintMomentum; + m_pidControllerActive = true; + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } } } -- cgit v1.1 From 86a2169d7343825c74ae271f637002377b92b438 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 16 Apr 2012 16:16:55 +0100 Subject: ubitODE + physmanager: - Revised use of ODE collisions categories and bits(flags) for better use as filters together with top spaces (for example physical prims are on topactivespace and not physical are on topstaticspace) - Added new world raycast with filters. This blocks calling thread with a timeout of 500ms waiting for heartbeat ode thread signal job done. - Don't let ode bodies being disabled for 2 long except for vehicles. This is necessary to detect when the object is at rest at top of other and that is removed. Assume that vehicles can be enabled by used action. --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 6 + OpenSim/Region/Physics/Manager/PhysicsScene.cs | 40 + .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 29 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 1069 +++++++++----------- .../UbitOdePlugin/ODERayCastRequestManager.cs | 326 ++++-- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 8 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 234 +++-- 7 files changed, 983 insertions(+), 729 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index be67204..b66d7f1 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -172,6 +172,12 @@ namespace OpenSim.Region.Physics.Manager public virtual bool Phantom { get; set; } + public virtual bool IsVolumeDtc + { + get { return false; } + set { return; } + } + public virtual byte PhysicsShapeType { get; set; } public abstract PrimitiveBaseShape Shape { set; } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index f2c0c28..d10a2aa 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -43,6 +43,34 @@ namespace OpenSim.Region.Physics.Manager public delegate void JointDeactivated(PhysicsJoint joint); public delegate void JointErrorMessage(PhysicsJoint joint, string message); // this refers to an "error message due to a problem", not "amount of joint constraint violation" + public enum RayFilterFlags:ushort + { + // the flags + water = 0x01, + land = 0x02, + agent = 0x04, + nonphysical = 0x08, + physical = 0x10, + phantom = 0x20, + volumedtc = 0x40, + + // ray cast colision control (may only work for meshs) + BackFaceCull = 0x4000, + ClosestHit = 0x8000, + + // some combinations + LSLPhanton = phantom | volumedtc, + PrimsNonPhantom = nonphysical | physical, + PrimsNonPhantomAgents = nonphysical | physical | agent, + + AllPrims = nonphysical | phantom | volumedtc | physical, + AllButLand = agent | nonphysical | physical | phantom | volumedtc, + + ClosestAndBackCull = ClosestHit | BackFaceCull, + + All = 0x3f + } + /// /// Contact result from a raycast. /// @@ -54,6 +82,8 @@ namespace OpenSim.Region.Physics.Manager public Vector3 Normal; } + + public abstract class PhysicsScene { // private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); @@ -280,6 +310,16 @@ namespace OpenSim.Region.Physics.Manager return new List(); } + public virtual object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) + { + return null; + } + + public virtual bool SuportsRaycastWorldFiltered() + { + return false; + } + public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){} public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { } public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 4266fda..b9bb06e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -115,12 +115,10 @@ namespace OpenSim.Region.Physics.OdePlugin private CollisionCategories m_collisionCategories = (CollisionCategories.Character); // Default, Collide with Other Geometries, spaces, bodies and characters. - private CollisionCategories m_collisionFlags = (CollisionCategories.Geom - | CollisionCategories.Space - | CollisionCategories.Body - | CollisionCategories.Character + private CollisionCategories m_collisionFlags = (CollisionCategories.Character + | CollisionCategories.Geom ); - // we do land collisions not ode | CollisionCategories.Land); + // we do land collisions not ode | CollisionCategories.Land); public IntPtr Body = IntPtr.Zero; private OdeScene _parent_scene; public IntPtr Shell = IntPtr.Zero; @@ -639,6 +637,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SetMomentum(Vector3 momentum) { + if (momentum.IsFinite()) + AddChange(changes.Momentum, momentum); } @@ -663,8 +663,8 @@ namespace OpenSim.Region.Physics.OdePlugin } Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); - d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); - d.GeomSetCollideBits(Shell, (int)m_collisionFlags); + d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories); + d.GeomSetCollideBits(Shell, (uint)m_collisionFlags); d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); @@ -759,7 +759,6 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.geom_name_map.Remove(Shell); _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); d.GeomDestroy(Shell); - _parent_scene.geom_name_map.Remove(Shell); Shell = IntPtr.Zero; } } @@ -1324,6 +1323,16 @@ namespace OpenSim.Region.Physics.OdePlugin } } + // for now momentum is actually velocity + private void changeMomentum(Vector3 newmomentum) + { + _velocity = newmomentum; + _target_velocity = newmomentum; + m_pidControllerActive = true; + if (Body != IntPtr.Zero) + d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); + } + private void donullchange() { } @@ -1395,6 +1404,10 @@ namespace OpenSim.Region.Physics.OdePlugin case changes.Size: changeSize((Vector3)arg); break; + + case changes.Momentum: + changeMomentum((Vector3)arg); + break; /* not in use for now case changes.Shape: changeShape((PrimitiveBaseShape)arg); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index f739183..32c4722 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -108,25 +108,29 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_waterHeight; private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - private int body_autodisable_frames = 20; + private int body_autodisable_frames = 5; + private int bodydisablecontrol = 0; + + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + // Default colide nonphysical don't try to colide with anything + private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; + + private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Land | + CollisionCategories.VolumeDtc); - private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom - | CollisionCategories.Space - | CollisionCategories.Body - | CollisionCategories.Character - ); // private bool m_collidesLand = true; private bool m_collidesWater; public bool m_returnCollisions; - private bool m_softcolide; private bool m_NoColide; // for now only for internal use for bad meshs - // Default we're a Geometry - private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); // Default, Collide with Other Geometries, spaces and Bodies - private CollisionCategories m_collisionFlags = m_default_collisionFlags; + private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; public bool m_disabled; @@ -179,6 +183,7 @@ namespace OpenSim.Region.Physics.OdePlugin public float primOOBradiusSQ; public d.Mass primdMass; // prim inertia information on it's own referencial float primMass; // prim own mass + float primVolume; // prim own volume; float _mass; // object mass acording to case private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb @@ -216,6 +221,14 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public override bool IsVolumeDtc + { + set { return; } + get { return m_isVolumeDetect; } + + } + + public override bool Phantom // this is not reliable for internal use { get { return m_fakeisphantom; } @@ -327,10 +340,7 @@ namespace OpenSim.Region.Physics.OdePlugin } if (m_colliderfilter == 0) - { - m_softcolide = false; m_iscolliding = false; - } else m_iscolliding = true; } @@ -422,6 +432,10 @@ namespace OpenSim.Region.Physics.OdePlugin public override Vector3 GeometricCenter { + // this is not real geometric center but a average of positions relative to root prim acording to + // http://wiki.secondlife.com/wiki/llGetGeometricCenter + // ignoring tortured prims details since sl also seems to ignore + // so no real use in doing it on physics get { return Vector3.Zero; @@ -949,7 +963,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_iscolliding = false; m_colliderfilter = 0; - m_softcolide = true; m_NoColide = false; hasOOBoffsetFromMesh = false; @@ -992,6 +1005,132 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionscore = 0; } + private void UpdateCollisionCatFlags() + { + if(m_isphysical && m_disabled) + { + m_collisionCategories = 0; + m_collisionFlags = 0; + } + + else if (m_isSelected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = 0; + } + + else if (m_isVolumeDetect) + { + m_collisionCategories = CollisionCategories.VolumeDtc; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; + else + m_collisionFlags = 0; + } + else if (m_isphantom) + { + m_collisionCategories = CollisionCategories.Phantom; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionFlags = m_default_collisionFlagsPhysical; + else + m_collisionFlags = m_default_collisionFlagsNotPhysical; + } + } + + private void ApplyCollisionCatFlags() + { + if (prim_geom != IntPtr.Zero) + { + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + { + if (m_isphysical && m_disabled) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = 0; + } + else + { + // preserve some + if (prm.m_isSelected) + { + prm.m_collisionCategories = CollisionCategories.Selected; + prm.m_collisionFlags = 0; + } + else if (prm.IsVolumeDtc) + { + prm.m_collisionCategories = CollisionCategories.VolumeDtc; + if (m_isphysical) + prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; + else + prm.m_collisionFlags = 0; + } + else if (prm.m_isphantom) + { + prm.m_collisionCategories = CollisionCategories.Phantom; + if (m_isphysical) + prm.m_collisionFlags = CollisionCategories.Land; + else + prm.m_collisionFlags = 0; + } + else + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + } + } + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); + } + } + } + } + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, 0); + d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); + } + } + } + } + private void createAMotor(Vector3 axis) { if (Body == IntPtr.Zero) @@ -1188,7 +1327,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(prim_geom, 0); if (m_isphysical) { - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); } else { @@ -1198,8 +1337,8 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } CalcPrimBodyData(); @@ -1296,6 +1435,7 @@ namespace OpenSim.Region.Physics.OdePlugin } prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; } else { @@ -1319,66 +1459,23 @@ namespace OpenSim.Region.Physics.OdePlugin { IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); prim.m_targetSpace = targetSpace; - d.GeomEnable(prim_geom); + collide_geom = IntPtr.Zero; } public void enableBodySoft() { + m_disabled = false; if (!childPrim && !m_isSelected) { if (m_isphysical && Body != IntPtr.Zero) { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - m_collisionFlags = CollisionCategories.Land; - } - else - { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prm.prim_geom); - } - } - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prim_geom); - } d.BodyEnable(Body); } } - m_disabled = false; - resetCollisionAccounting(); // this sets m_disable to false + resetCollisionAccounting(); } private void disableBodySoft() @@ -1388,45 +1485,12 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical && Body != IntPtr.Zero) { - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prm.prim_geom); - } - } - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomDisable(prim_geom); - } - + if (m_isSelected) + m_collisionFlags = CollisionCategories.Selected; + else + m_collisionCategories = 0; + m_collisionFlags = 0; + ApplyCollisionCatFlags(); d.BodyDisable(Body); } } @@ -1566,7 +1630,6 @@ namespace OpenSim.Region.Physics.OdePlugin // d.BodySetAngularDampingThreshold(Body, 0.001f); d.BodySetDamping(Body, .002f, .002f); - if (m_targetSpace != IntPtr.Zero) { _parent_scene.waitForSpaceUnlock(m_targetSpace); @@ -1588,6 +1651,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceSetSublevel(m_targetSpace, 3); d.SpaceSetCleanup(m_targetSpace, false); d.SpaceAdd(m_targetSpace, prim_geom); + + d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(m_targetSpace, 0); collide_geom = m_targetSpace; } @@ -1619,38 +1689,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceAdd(m_targetSpace, prm.prim_geom); } - if (m_isSelected || m_disabled) - { - prm.m_collisionCategories &= ~CollisionCategories.Body; - prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); - d.GeomDisable(prm.prim_geom); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - prm.m_collisionCategories = 0; - prm.m_collisionFlags = CollisionCategories.Land; - } - else - { - prm.m_collisionCategories |= CollisionCategories.Body; - prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - d.GeomEnable(prm.prim_geom); - } - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - d.GeomEnable(prm.prim_geom); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); - } prm.m_collisionscore = 0; if(!m_disabled) @@ -1666,45 +1704,21 @@ namespace OpenSim.Region.Physics.OdePlugin createAMotor(m_angularlock); } + m_collisionscore = 0; + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + if (m_isSelected || m_disabled) { - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); - - d.GeomDisable(prim_geom); d.BodyDisable(Body); } else { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - m_collisionFlags = CollisionCategories.Land; - } - else - { - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - } - d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); } - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - - m_collisionscore = 0; - - m_softcolide = true; _parent_scene.addActivePrim(this); _parent_scene.addActiveGroups(this); } @@ -1714,8 +1728,22 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) { _parent_scene.remActivePrim(this); - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + collide_geom = IntPtr.Zero; + + if (m_disabled) + m_collisionCategories = 0; + else if (m_isSelected) + m_collisionCategories = CollisionCategories.Selected; + else if (m_isVolumeDetect) + m_collisionCategories = CollisionCategories.VolumeDtc; + else if (m_isphantom) + m_collisionCategories = CollisionCategories.Phantom; + else + m_collisionCategories = CollisionCategories.Geom; + + m_collisionFlags = 0; + if (prim_geom != IntPtr.Zero) { if (m_NoColide) @@ -1725,8 +1753,8 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } UpdateDataFromGeom(); d.GeomSetBody(prim_geom, IntPtr.Zero); @@ -1740,8 +1768,18 @@ namespace OpenSim.Region.Physics.OdePlugin foreach (OdePrim prm in childrenPrim) { _parent_scene.remActivePrim(prm); - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; + + if (prm.m_isSelected) + prm.m_collisionCategories = CollisionCategories.Selected; + else if (prm.m_isVolumeDetect) + prm.m_collisionCategories = CollisionCategories.VolumeDtc; + else if (prm.m_isphantom) + prm.m_collisionCategories = CollisionCategories.Phantom; + else + prm.m_collisionCategories = CollisionCategories.Geom; + + prm.m_collisionFlags = 0; + if (prm.prim_geom != IntPtr.Zero) { if (prm.m_NoColide) @@ -1751,8 +1789,8 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); } prm.UpdateDataFromGeom(); SetInStaticSpace(prm); @@ -2292,6 +2330,7 @@ namespace OpenSim.Region.Physics.OdePlugin // keep using basic shape mass for now volume = CalculatePrimVolume(); + primVolume = volume; primMass = m_density * volume; if (primMass <= 0) @@ -2515,12 +2554,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetQuaternion(prim_geom, ref myrot); if (!m_isphysical) + { SetInStaticSpace(this); - } - - if (m_isphysical && Body == IntPtr.Zero) - { - MakeBody(); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + else + MakeBody(); } } @@ -2602,86 +2642,12 @@ namespace OpenSim.Region.Physics.OdePlugin } } - private void changePhantomStatus(bool newval) { m_isphantom = newval; - if (m_isSelected) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); - } - else - { - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - if (m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; // should never happen - } - - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; - } - } - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - if(!m_isSelected) - d.GeomEnable(prm.prim_geom); - } - } - } - - if (!m_disabled && prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - if(!m_isSelected) - d.GeomEnable(prim_geom); - } + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); } private void changeSelectedStatus(bool newval) @@ -2714,7 +2680,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_delaySelect || m_isphysical) { m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + m_collisionFlags = 0; if (!childPrim) { @@ -2733,10 +2699,9 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); } - d.GeomDisable(prm.prim_geom); } prm.m_delaySelect = false; } @@ -2748,13 +2713,23 @@ namespace OpenSim.Region.Physics.OdePlugin { d.GeomSetCategoryBits(prim_geom, 0); d.GeomSetCollideBits(prim_geom, 0); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, 0); + d.GeomSetCollideBits(collide_geom, 0); + } + } else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); + } } - d.GeomDisable(prim_geom); } m_delaySelect = false; @@ -2769,75 +2744,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim && Body != IntPtr.Zero && !m_disabled) d.BodyEnable(Body); - if (m_isphantom && !m_isVolumeDetect) - { - m_collisionCategories = 0; - if(m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; - } - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; - - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; - } - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prm.prim_geom); - } - prm.m_delaySelect = false; - prm.m_softcolide = true; - } - } - - if (!m_disabled && prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } - d.GeomEnable(prim_geom); - } + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); m_delaySelect = false; - m_softcolide = true; } resetCollisionAccounting(); @@ -2890,7 +2800,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (givefakepos < 0) givefakepos = 0; // changeSelectedStatus(); - m_softcolide = true; resetCollisionAccounting(); } @@ -2951,7 +2860,6 @@ namespace OpenSim.Region.Physics.OdePlugin givefakeori--; if (givefakeori < 0) givefakeori = 0; - m_softcolide = true; resetCollisionAccounting(); } @@ -3022,7 +2930,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (givefakeori < 0) givefakeori = 0; - m_softcolide = true; resetCollisionAccounting(); } @@ -3111,17 +3018,25 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetQuaternion(prim_geom, ref myrot); } - if (chp) + if (m_isphysical) { - if (parent != null) + if (chp) { - parent.MakeBody(); + if (parent != null) + { + parent.MakeBody(); + } } + else + MakeBody(); } + else - MakeBody(); + { + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } - m_softcolide = true; resetCollisionAccounting(); } @@ -3142,18 +3057,8 @@ namespace OpenSim.Region.Physics.OdePlugin { m_collidesWater = newval; - if (prim_geom != IntPtr.Zero && !m_isphantom) - { - if (m_collidesWater) - { - m_collisionFlags |= CollisionCategories.Water; - } - else - { - m_collisionFlags &= ~CollisionCategories.Water; - } - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); } private void changeSetTorque(Vector3 newtorque) @@ -3240,6 +3145,8 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeVolumedetetion(bool newVolDtc) { m_isVolumeDetect = newVolDtc; + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); } protected void changeBuilding(bool newbuilding) @@ -3320,288 +3227,306 @@ namespace OpenSim.Region.Physics.OdePlugin public void Move() { if (!childPrim && m_isphysical && Body != IntPtr.Zero && - !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) + !m_disabled && !m_isSelected && !m_building && !m_outbounds) // !m_disabled && !m_isSelected && !m_building && !m_outbounds) { -// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 - - float timestep = _parent_scene.ODE_STEPSIZE; + // if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 - // check outside region - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - - if (lpos.Z < -100 || lpos.Z > 100000f) + if (d.BodyIsEnabled(Body)) { - m_outbounds = true; + float timestep = _parent_scene.ODE_STEPSIZE; - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; + // check outside region + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; - base.RequestPhysicsterseUpdate(); + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - return; - } + base.RequestPhysicsterseUpdate(); - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if(lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if(lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; - if(m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; + if (m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - base.RequestPhysicsterseUpdate(); - return; - } + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; - float fx = 0; - float fy = 0; - float fz = 0; + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(); - } - else - { - float m_mass = _mass; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + return; + } - // fz = 0f; - //m_log.Info(m_collisionFlags.ToString()); - if (m_usePID) + else { - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + float fx = 0; + float fy = 0; + float fz = 0; - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } + float m_mass = _mass; - if ((PID_G - m_PIDTau) <= 0) + // fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + if (m_usePID) { - PID_G = m_PIDTau + 1; - } - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } // end if (m_usePID) + // if velocity is zero, use position control; otherwise, velocity control - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID) - { - //Console.WriteLine("Hover " + Name); + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; - // If we're using the PID controller, then we have no gravity + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - // no lock; for now it's only called from within Simulate() + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } // end if (m_usePID) - if ((m_PIDTau < 1)) + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID) { - PID_G = PID_G / m_PIDTau; - } + //Console.WriteLine("Hover " + Name); - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } + // If we're using the PID controller, then we have no gravity - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); + // no lock; for now it's only called from within Simulate() - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; - } // end switch (m_PIDHoverType) + } // end switch (m_PIDHoverType) - _target_velocity = - new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); - // if velocity is zero, use position control; otherwise, velocity control + // if velocity is zero, use position control; otherwise, velocity control - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - // ? d.BodyAddForce(Body, 0, 0, fz); - return; + // We're flying and colliding with something + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } } else { - _zeroFlag = false; - - // We're flying and colliding with something - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; } - } - else - { - float b = (1.0f - m_buoyancy); - fx = _parent_scene.gravityx * b; - fy = _parent_scene.gravityy * b; - fz = _parent_scene.gravityz * b; - } - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; - m_forceacc = Vector3.Zero; + m_forceacc = Vector3.Zero; - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - d.BodyAddForce(Body, fx, fy, fz); - //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } - Vector3 trq; + Vector3 trq; + + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } - trq = _torque; - trq += m_angularForceacc; - m_angularForceacc = Vector3.Zero; - if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) - { - d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); } + } + else // body disabled + { + // let vehicles sleep + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + return; + if (++bodydisablecontrol < 20) + return; + + bodydisablecontrol = 0; + d.BodyEnable(Body); + return; } } else diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 4b3f83b..e66580d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -30,10 +30,11 @@ using System.Collections.Generic; using System.Reflection; using System.Runtime.InteropServices; using System.Text; -using OpenMetaverse; +using OpenSim.Framework; using OpenSim.Region.Physics.Manager; using OdeAPI; using log4net; +using OpenMetaverse; namespace OpenSim.Region.Physics.OdePlugin { @@ -54,9 +55,11 @@ namespace OpenSim.Region.Physics.OdePlugin /// private OdeScene m_scene; - IntPtr ray; + IntPtr ray; // the ray. we only need one for our lifetime private const int ColisionContactGeomsPerTest = 5; + private const int DefaultMaxCount = 25; + private const int MaxTimePerCallMS = 30; /// /// ODE near callback delegate @@ -64,19 +67,22 @@ namespace OpenSim.Region.Physics.OdePlugin private d.NearCallback nearCallback; private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); private List m_contactResults = new List(); + private RayFilterFlags CurrentRayFilter; + private int CurrentMaxCount; public ODERayCastRequestManager(OdeScene pScene) { m_scene = pScene; nearCallback = near; ray = d.CreateRay(IntPtr.Zero, 1.0f); + d.GeomSetCategoryBits(ray,0); } /// - /// Queues a raycast + /// Queues request for a raycast to all world /// /// Origin of Ray - /// Ray normal + /// Ray direction /// Ray length /// Return method to send the results public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod) @@ -84,14 +90,22 @@ namespace OpenSim.Region.Physics.OdePlugin ODERayRequest req = new ODERayRequest(); req.geom = IntPtr.Zero; req.callbackMethod = retMethod; - req.Count = 0; + req.Count = DefaultMaxCount; req.length = length; req.Normal = direction; req.Origin = position; + req.filter = RayFilterFlags.AllButLand; m_PendingRequests.Enqueue(req); } + /// + /// Queues request for a raycast to particular part + /// + /// Origin of Ray + /// Ray direction + /// Ray length + /// Return method to send the results public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod) { ODERayRequest req = new ODERayRequest(); @@ -100,7 +114,8 @@ namespace OpenSim.Region.Physics.OdePlugin req.length = length; req.Normal = direction; req.Origin = position; - req.Count = 0; + req.Count = DefaultMaxCount; + req.filter = RayFilterFlags.AllButLand; m_PendingRequests.Enqueue(req); } @@ -110,10 +125,11 @@ namespace OpenSim.Region.Physics.OdePlugin ODERayRequest req = new ODERayRequest(); req.geom = IntPtr.Zero; req.callbackMethod = retMethod; - req.Count = 0; + req.Count = DefaultMaxCount; req.length = length; req.Normal = direction; req.Origin = position; + req.filter = RayFilterFlags.AllButLand; m_PendingRequests.Enqueue(req); } @@ -126,7 +142,8 @@ namespace OpenSim.Region.Physics.OdePlugin req.length = length; req.Normal = direction; req.Origin = position; - req.Count = 0; + req.Count = DefaultMaxCount; + req.filter = RayFilterFlags.AllButLand; m_PendingRequests.Enqueue(req); } @@ -148,6 +165,22 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = count; + req.filter = RayFilterFlags.AllButLand; + + m_PendingRequests.Enqueue(req); + } + + + public void QueueRequest(Vector3 position, Vector3 direction, float length, int count,RayFilterFlags filter , RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + req.filter = filter; m_PendingRequests.Enqueue(req); } @@ -161,6 +194,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = count; + req.filter = RayFilterFlags.AllButLand; m_PendingRequests.Enqueue(req); } @@ -174,6 +208,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = count; + req.filter = RayFilterFlags.AllButLand; m_PendingRequests.Enqueue(req); } @@ -187,6 +222,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = count; + req.filter = RayFilterFlags.AllButLand; m_PendingRequests.Enqueue(req); } @@ -197,63 +233,104 @@ namespace OpenSim.Region.Physics.OdePlugin /// Time in MS the raycasts took to process. public int ProcessQueuedRequests() { - int time = System.Environment.TickCount; if (m_PendingRequests.Count <= 0) return 0; - if (m_scene.ContactgeomsArray == IntPtr.Zero) // oops something got wrong or scene isn't ready still + if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero) + // oops something got wrong or scene isn't ready still { m_PendingRequests.Clear(); return 0; } - ODERayRequest req; + int time = Util.EnvironmentTickCount(); - int i = 50; // arbitary limit of processed tests per frame + ODERayRequest req; + int closestHit; + int backfacecull; + CollisionCategories catflags; - while(m_PendingRequests.Dequeue(out req)) + while (m_PendingRequests.Dequeue(out req)) { - if (req.geom == IntPtr.Zero) - doSpaceRay(req); - else - doGeomRay(req); - if(--i < 0) - break; + if (req.callbackMethod != null) + { + CurrentRayFilter = req.filter; + CurrentMaxCount = req.Count; + + closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); + backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); + + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + d.GeomRaySetParams(ray, 0, backfacecull); + d.GeomRaySetClosestHit(ray, closestHit); + + if (req.callbackMethod is RaycastCallback) + // if we only want one get only one per colision pair saving memory + CurrentRayFilter |= RayFilterFlags.ClosestHit; + + if (req.geom == IntPtr.Zero) + { + // translate ray filter to colision flags + catflags = 0; + if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0) + catflags |= CollisionCategories.VolumeDtc; + if ((CurrentRayFilter & RayFilterFlags.phantom) != 0) + catflags |= CollisionCategories.Phantom; + if ((CurrentRayFilter & RayFilterFlags.agent) != 0) + catflags |= CollisionCategories.Character; + if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0) + catflags |= CollisionCategories.Geom; + if ((CurrentRayFilter & RayFilterFlags.land) != 0) + catflags |= CollisionCategories.Land; + if ((CurrentRayFilter & RayFilterFlags.water) != 0) + catflags |= CollisionCategories.Water; + + if (catflags != 0) + doSpaceRay(req); + } + else + { + // if we select a geom don't use filters + d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); + doGeomRay(req); + } + } + + if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS) + break; } lock (m_contactResults) m_contactResults.Clear(); - return System.Environment.TickCount - time; + return Util.EnvironmentTickCountSubtract(time); } /// - /// Method that actually initiates the raycast with full top space + /// Method that actually initiates the raycast with spaces /// /// - private void doSpaceRay(ODERayRequest req) - { - // Create the ray -// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length); - d.GeomRaySetLength(ray, req.length); - d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + /// - // Collide test - d.SpaceCollide2(m_scene.TopSpace, ray, IntPtr.Zero, nearCallback); - - // Remove Ray -// d.GeomDestroy(ray); + private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton; + private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; - if (req.callbackMethod == null) - return; + private void doSpaceRay(ODERayRequest req) + { + // Collide tests + if ((CurrentRayFilter & FilterActiveSpace) != 0) + d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); if (req.callbackMethod is RaycastCallback) { // Define default results bool hitYN = false; uint hitConsumerID = 0; - float distance = 999999999999f; - Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + float distance = float.MaxValue; + Vector3 closestcontact = Vector3.Zero; Vector3 snormal = Vector3.Zero; // Find closest contact and object. @@ -261,25 +338,30 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (ContactResult cResult in m_contactResults) { - if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + if(cResult.Depth < distance) { closestcontact = cResult.Pos; hitConsumerID = cResult.ConsumerID; distance = cResult.Depth; - hitYN = true; snormal = cResult.Normal; } } m_contactResults.Clear(); } - + + if (distance > 0 && distance < float.MaxValue) + hitYN = true; ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); } else { - ((RayCallback)req.callbackMethod)(m_contactResults); + List cresult = new List(m_contactResults.Count); lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); m_contactResults.Clear(); + } + ((RayCallback)req.callbackMethod)(cresult); } } @@ -289,27 +371,16 @@ namespace OpenSim.Region.Physics.OdePlugin /// private void doGeomRay(ODERayRequest req) { - // Create the ray -// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length); - d.GeomRaySetLength(ray, req.length); - d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); - // Collide test - d.SpaceCollide2(req.geom, ray, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test - - // Remove Ray -// d.GeomDestroy(ray); - - if (req.callbackMethod == null) - return; + d.SpaceCollide2(ray, req.geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test if (req.callbackMethod is RaycastCallback) { // Define default results bool hitYN = false; uint hitConsumerID = 0; - float distance = 999999999999f; - Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + float distance = float.MaxValue; + Vector3 closestcontact = Vector3.Zero; Vector3 snormal = Vector3.Zero; // Find closest contact and object. @@ -317,25 +388,31 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (ContactResult cResult in m_contactResults) { - if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + if(cResult.Depth < distance ) { closestcontact = cResult.Pos; hitConsumerID = cResult.ConsumerID; distance = cResult.Depth; - hitYN = true; snormal = cResult.Normal; } } m_contactResults.Clear(); } + if (distance > 0 && distance < float.MaxValue) + hitYN = true; + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); } else { - ((RayCallback)req.callbackMethod)(m_contactResults); + List cresult = new List(m_contactResults.Count); lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); m_contactResults.Clear(); + } + ((RayCallback)req.callbackMethod)(cresult); } } @@ -350,20 +427,16 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } - // This is the standard Near. g2 is the ray + // This is the standard Near. g1 is the ray private void near(IntPtr space, IntPtr g1, IntPtr g2) { - //Don't test against heightfield Geom, or you'll be sorry! - // Exclude heightfield geom - - if (g1 == IntPtr.Zero || g1 == g2) + if (g2 == IntPtr.Zero || g1 == g2) return; - if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + if (m_contactResults.Count >= CurrentMaxCount) return; - // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms. - if (d.GeomIsSpace(g1)) + if (d.GeomIsSpace(g2)) { try { @@ -381,10 +454,6 @@ namespace OpenSim.Region.Physics.OdePlugin { count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); } - catch (SEHException) - { - m_log.Error("[PHYSICS Ray]: 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."); - } catch (Exception e) { m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); @@ -394,31 +463,116 @@ namespace OpenSim.Region.Physics.OdePlugin if (count == 0) return; - PhysicsActor p1 = null; + uint ID = 0; + PhysicsActor p2 = null; + + m_scene.actor_name_map.TryGetValue(g2, out p2); + + if (p2 == null) + { + string name; + + if (!m_scene.geom_name_map.TryGetValue(g2, out name)) + return; + + if (name == "Terrain") + { + // land colision + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + } + else if (name == "Water") + { + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + } + else + return; + } + else + { + if (p2 is OdePrim) + { + RayFilterFlags thisFlags; + + if (p2.IsPhysical) + thisFlags = RayFilterFlags.physical; + else + thisFlags = RayFilterFlags.nonphysical; - if (g1 != IntPtr.Zero) - m_scene.actor_name_map.TryGetValue(g1, out p1); + if (p2.Phantom) + thisFlags |= RayFilterFlags.phantom; + + if (p2.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; + + if ((thisFlags & CurrentRayFilter) == 0) + return; + + ID = ((OdePrim)p2).m_localID; + } + else if (p2 is OdeCharacter) + { + if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + return; + else + ID = ((OdeCharacter)p2).m_localID; + } + else //?? + return; + } d.ContactGeom curcontact = new d.ContactGeom(); - // Loop over contacts, build results. - for (int i = 0; i < count; i++) + + // closestHit for now only works for meshs, so must do it for others + if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) { - if (!GetCurContactGeom(i, ref curcontact)) - break; - if (p1 != null) { - if (p1 is OdePrim) + // Loop all contacts, build results. + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Depth = curcontact.depth; + collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, + curcontact.normal.Z); + lock (m_contactResults) + { + m_contactResults.Add(collisionresult); + if (m_contactResults.Count >= CurrentMaxCount) + return; + } + } + } + else + { + // keep only closest contact + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Depth = float.MaxValue; + + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + + if (curcontact.depth < collisionresult.Depth) { - ContactResult collisionresult = new ContactResult(); - - collisionresult.ConsumerID = ((OdePrim)p1).m_localID; collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); collisionresult.Depth = curcontact.depth; collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, curcontact.normal.Z); - lock (m_contactResults) - m_contactResults.Add(collisionresult); } } + + if (collisionresult.Depth != float.MaxValue) + { + lock (m_contactResults) + m_contactResults.Add(collisionresult); + } } } @@ -428,6 +582,11 @@ namespace OpenSim.Region.Physics.OdePlugin internal void Dispose() { m_scene = null; + if (ray != IntPtr.Zero) + { + d.GeomDestroy(ray); + ray = IntPtr.Zero; + } } } @@ -439,5 +598,6 @@ namespace OpenSim.Region.Physics.OdePlugin public int Count; public float length; public object callbackMethod; + public RayFilterFlags filter; } } \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index f5129cb..0e4961b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -889,13 +889,13 @@ namespace OdeAPI public static extern IntPtr GeomGetBody(IntPtr geom); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity] - public static extern int GeomGetCategoryBits(IntPtr geom); + public static extern uint GeomGetCategoryBits(IntPtr geom); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity] public static extern IntPtr GeomGetClassData(IntPtr geom); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity] - public static extern int GeomGetCollideBits(IntPtr geom); + public static extern uint GeomGetCollideBits(IntPtr geom); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity] public static extern GeomClassID GeomGetClass(IntPtr geom); @@ -1086,10 +1086,10 @@ namespace OdeAPI public static extern void GeomSetBody(IntPtr geom, IntPtr body); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetCategoryBits(IntPtr geom, int bits); + public static extern void GeomSetCategoryBits(IntPtr geom, uint bits); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetCollideBits(IntPtr geom, int bits); + public static extern void GeomSetCollideBits(IntPtr geom, uint bits); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 3e0ccef..7632e25 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -60,19 +60,31 @@ namespace OpenSim.Region.Physics.OdePlugin public int lastframe; } + // colision flags of things others can colide with + // rays, sensors, probes removed since can't be colided with + // The top space where things are placed provided further selection + // ie physical are in active space nonphysical in static + // this should be exclusive as possible + [Flags] - public enum CollisionCategories : int + public enum CollisionCategories : uint { Disabled = 0, - Geom = 0x00000001, - Body = 0x00000002, - Space = 0x00000004, - Character = 0x00000008, - Land = 0x00000010, - Water = 0x00000020, - Wind = 0x00000040, - Sensor = 0x00000080, - Selected = 0x00000100 + //by 'things' types + Space = 0x01, + Geom = 0x02, // aka prim/part + Character = 0x04, + Land = 0x08, + Water = 0x010, + + // by state + Phantom = 0x01000, + VolumeDtc = 0x02000, + Selected = 0x04000, + NoShape = 0x08000, + + + All = 0xffffffff } /// @@ -116,6 +128,7 @@ namespace OpenSim.Region.Physics.OdePlugin Acceleration, Force, Torque, + Momentum, AddForce, AddAngForce, @@ -186,7 +199,9 @@ namespace OpenSim.Region.Physics.OdePlugin private float waterlevel = 0f; private int framecount = 0; - internal IntPtr WaterGeom; + private IntPtr WaterGeom = IntPtr.Zero; + private IntPtr WaterHeightmapData = IntPtr.Zero; + private GCHandle WaterMapHandler = new GCHandle(); public float avPIDD = 2200f; // make it visible public float avPIDP = 900f; // make it visible @@ -213,9 +228,8 @@ namespace OpenSim.Region.Physics.OdePlugin // public int geomCrossingFailuresBeforeOutofbounds = 6; - public int bodyFramesAutoDisable = 20; + public int bodyFramesAutoDisable = 5; - private float[] _watermap; private d.NearCallback nearCallback; @@ -350,7 +364,7 @@ namespace OpenSim.Region.Physics.OdePlugin // i must RtC#FM } - d.HashSpaceSetLevels(TopSpace, -2, 8); // cell sizes from .25 to 256 ?? need check what this really does + d.HashSpaceSetLevels(TopSpace, -2, 8); d.HashSpaceSetLevels(ActiveSpace, -2, 8); d.HashSpaceSetLevels(StaticSpace, -2, 8); @@ -358,13 +372,27 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceSetSublevel(ActiveSpace, 1); d.SpaceSetSublevel(StaticSpace, 1); + d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(ActiveSpace, 0); + d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(StaticSpace, 0); + contactgroup = d.JointGroupCreate(0); //contactgroup d.WorldSetAutoDisableFlag(world, false); } - - _watermap = new float[258 * 258]; } // Initialize the mesh plugin @@ -374,15 +402,18 @@ namespace OpenSim.Region.Physics.OdePlugin // checkThread(); mesher = meshmerizer; m_config = config; - +/* string ode_config = d.GetConfiguration("ODE"); - m_log.WarnFormat("ODE configuration: {0}", ode_config); - - if (ode_config.Contains("ODE_Ubit")) + if (ode_config != null && ode_config != "") { - OdeUbitLib = true; - } + m_log.WarnFormat("ODE configuration: {0}", ode_config); + if (ode_config.Contains("ODE_Ubit")) + { + OdeUbitLib = true; + } + } +*/ /* if (region != null) { @@ -518,6 +549,15 @@ namespace OpenSim.Region.Physics.OdePlugin waitForSpaceUnlock(newspace); d.SpaceSetSublevel(newspace, 2); d.HashSpaceSetLevels(newspace, -2, 8); + d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(newspace, 0); + staticPrimspace[i, j] = newspace; } // let this now be real maximum values @@ -1745,8 +1785,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_rayCastManager.ProcessQueuedRequests(); - - statray += Util.EnvironmentTickCountSubtract(statstart); collision_optimized(); statcol += Util.EnvironmentTickCountSubtract(statstart); @@ -2125,14 +2163,14 @@ namespace OpenSim.Region.Physics.OdePlugin RegionTerrain.Remove(pOffset); if (GroundGeom != IntPtr.Zero) { + d.GeomDestroy(GroundGeom); + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) { TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); TerrainHeightFieldHeights.Remove(GroundGeom); } - d.SpaceRemove(StaticSpace, GroundGeom); - d.GeomDestroy(GroundGeom); } } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); @@ -2147,8 +2185,8 @@ namespace OpenSim.Region.Physics.OdePlugin GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); if (GroundGeom != IntPtr.Zero) { - d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, 0); } geom_name_map[GroundGeom] = "Terrain"; @@ -2236,14 +2274,15 @@ namespace OpenSim.Region.Physics.OdePlugin RegionTerrain.Remove(pOffset); if (GroundGeom != IntPtr.Zero) { + d.GeomDestroy(GroundGeom); + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) { - TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated) + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); TerrainHeightFieldHeights.Remove(GroundGeom); } - d.SpaceRemove(StaticSpace, GroundGeom); - d.GeomDestroy(GroundGeom); } } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); @@ -2263,8 +2302,8 @@ namespace OpenSim.Region.Physics.OdePlugin GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1); if (GroundGeom != IntPtr.Zero) { - d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, 0); } geom_name_map[GroundGeom] = "Terrain"; @@ -2359,57 +2398,76 @@ namespace OpenSim.Region.Physics.OdePlugin public void randomizeWater(float baseheight) { - const uint heightmapWidth = m_regionWidth + 2; - const uint heightmapHeight = m_regionHeight + 2; - const uint heightmapWidthSamples = m_regionWidth + 2; - const uint heightmapHeightSamples = m_regionHeight + 2; + const uint heightmapWidth = Constants.RegionSize + 2; + const uint heightmapHeight = Constants.RegionSize + 2; + const uint heightmapWidthSamples = heightmapWidth + 1; + const uint heightmapHeightSamples = heightmapHeight + 1; + const float scale = 1.0f; const float offset = 0.0f; - const float thickness = 2.9f; const int wrap = 0; - for (int i = 0; i < (258 * 258); i++) + float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples]; + + float maxheigh = float.MinValue; + float minheigh = float.MaxValue; + float val; + for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++) { - _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); - // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); + + val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f); + _watermap[i] = val; + if (maxheigh < val) + maxheigh = val; + if (minheigh > val) + minheigh = val; } + float thickness = minheigh; + lock (OdeLock) { if (WaterGeom != IntPtr.Zero) { - d.SpaceRemove(StaticSpace, WaterGeom); + d.GeomDestroy(WaterGeom); + d.GeomHeightfieldDataDestroy(WaterHeightmapData); + WaterGeom = IntPtr.Zero; + WaterHeightmapData = IntPtr.Zero; + if(WaterMapHandler.IsAllocated) + WaterMapHandler.Free(); } - IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, + + WaterHeightmapData = d.GeomHeightfieldDataCreate(); + + WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned); + + d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight, (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, offset, thickness, wrap); - d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); - WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); + d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh); + WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1); if (WaterGeom != IntPtr.Zero) { - d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); - d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); + d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water)); + d.GeomSetCollideBits(WaterGeom, 0); - } - geom_name_map[WaterGeom] = "Water"; + geom_name_map[WaterGeom] = "Water"; - d.Matrix3 R = new d.Matrix3(); + d.Matrix3 R = new d.Matrix3(); - Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); - Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); - q1 = q1 * q2; - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); + q1 = q1 * q2; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); - d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); - d.GeomSetRotation(WaterGeom, ref R); - d.GeomSetPosition(WaterGeom, 128, 128, 0); - + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(WaterGeom, ref R); + d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0); + } } - } public override void Dispose() @@ -2427,11 +2485,34 @@ namespace OpenSim.Region.Physics.OdePlugin } } + if (TerrainHeightFieldHeightsHandlers.Count > 0) + { + foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) + { + if (gch.IsAllocated) + gch.Free(); + } + } + + if (WaterGeom != IntPtr.Zero) + { + d.GeomDestroy(WaterGeom); + WaterGeom = IntPtr.Zero; + if (WaterHeightmapData != IntPtr.Zero) + d.GeomHeightfieldDataDestroy(WaterHeightmapData); + WaterHeightmapData = IntPtr.Zero; + + if (WaterMapHandler.IsAllocated) + WaterMapHandler.Free(); + } + + if (ContactgeomsArray != IntPtr.Zero) Marshal.FreeHGlobal(ContactgeomsArray); if (GlobalContactsArray != IntPtr.Zero) Marshal.FreeHGlobal(GlobalContactsArray); + d.WorldDestroy(world); //d.CloseODE(); } @@ -2502,6 +2583,35 @@ namespace OpenSim.Region.Physics.OdePlugin return new List(ourResults); } + public override bool SuportsRaycastWorldFiltered() + { + return true; + } + + public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) + { + object SyncObject = new object(); + List ourresults = new List(); + + RayCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourresults = results; + Monitor.PulseAll(SyncObject); + } + }; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod); + if (!Monitor.Wait(SyncObject, 500)) + return null; + else + return ourresults; + } + } + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) { if (retMethod != null && actor !=null) -- cgit v1.1 From 03139f07d7e5780dbe274e32911b4bf85d801ad8 Mon Sep 17 00:00:00 2001 From: Melanie Date: Mon, 16 Apr 2012 17:32:30 +0200 Subject: Downgrade an error log message to info because there is nothing we can do if an asset is damaged so it should not spew red ink. --- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 2 +- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index 75fa1ef..825b858 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -335,7 +335,7 @@ namespace OpenSim.Region.Physics.Meshing if (primShape.SculptData.Length <= 0) { - m_log.ErrorFormat("[MESH]: asset data for {0} is zero length", primName); + m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName); return false; } diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 8e903e8..f002bba 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -339,7 +339,7 @@ namespace OpenSim.Region.Physics.Meshing if (primShape.SculptData.Length <= 0) { - m_log.ErrorFormat("[MESH]: asset data for {0} is zero length", primName); + m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName); return false; } -- cgit v1.1 From 04ed5519a5ad265794a0768a6a3c9e4e0fdf1a6c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 16 Apr 2012 17:14:31 +0100 Subject: chODE bug fix --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index 1f1ba95..c53ccec 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -921,6 +921,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_haveTaintMomentum = true; _parent_scene.AddPhysicsActorTaint(this); } + else + m_log.Warn("[PHYSICS] !isFinite momentum"); } @@ -1337,7 +1339,6 @@ namespace OpenSim.Region.Physics.OdePlugin { lock (m_syncRoot) { - if (m_tainted_isPhysical != m_isPhysical) { if (m_tainted_isPhysical) @@ -1379,9 +1380,9 @@ namespace OpenSim.Region.Physics.OdePlugin { d.GeomDestroy(Shell); } - catch (System.AccessViolationException) + catch (Exception e) { - m_log.Error("[PHYSICS]: PrimGeom dead"); + m_log.ErrorFormat("[PHYSICS]: Failed to destroy character shell {0}",e.Message); } // Remove any old entries //string tShell; @@ -1428,10 +1429,10 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); - _position.X = m_taintPosition.X; - _position.Y = m_taintPosition.Y; - _position.Z = m_taintPosition.Z; } + _position.X = m_taintPosition.X; + _position.Y = m_taintPosition.Y; + _position.Z = m_taintPosition.Z; } if (m_haveTaintMomentum) @@ -1440,7 +1441,8 @@ namespace OpenSim.Region.Physics.OdePlugin _velocity = m_taintMomentum; _target_velocity = m_taintMomentum; m_pidControllerActive = true; - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + if (Body != IntPtr.Zero) + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); } } } -- cgit v1.1 From fff5c61ae83ad54eaff640d2868186d275f3dd62 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 16 Apr 2012 17:47:17 +0100 Subject: chODE: if character velocity > 50m/s apply breaks. In free fall this will give a terminal velocity +- 60m/s --- OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs index c53ccec..ec717d7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs @@ -1122,9 +1122,18 @@ namespace OpenSim.Region.Physics.OdePlugin } // end add Kitto Flora } + + if (vel.X * vel.X + vel.Y * vel.Y + vel.Z * vel.Z > 2500.0f) // 50ms apply breaks + { + float breakfactor = 0.16f * m_mass; // will give aprox 60m/s terminal velocity at free fall + vec.X -= breakfactor * vel.X; + vec.Y -= breakfactor * vel.Y; + vec.Z -= breakfactor * vel.Z; + } + if (vec.IsFinite()) { - if (!vec.ApproxEquals(Vector3.Zero, 0.02f)) // 0.01 allows 0.002 !! + if (vec.LengthSquared() > 0.0004f) // 0.01 allows 0.002 !! { //Console.WriteLine("DF 2"); // ## -- cgit v1.1 From 6480b72eda967d6166cb8a64c5bca20c7841358c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 16 Apr 2012 19:44:02 +0100 Subject: ubitODE: - fix remove characters from default raycasts filters as older code (or camera is very odd) - Slow down avatar if velocity is higher than 50m/s as in chODE --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 8 ++++++++ .../Physics/UbitOdePlugin/ODERayCastRequestManager.cs | 16 ++++++++-------- 2 files changed, 16 insertions(+), 8 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b9bb06e..3185aad 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -990,6 +990,14 @@ namespace OpenSim.Region.Physics.OdePlugin // end add Kitto Flora } + if (vel.X * vel.X + vel.Y * vel.Y + vel.Z * vel.Z > 2500.0f) // 50m/s apply breaks + { + float breakfactor = 0.16f * m_mass; // will give aprox 60m/s terminal velocity at free fall + vec.X -= breakfactor * vel.X; + vec.Y -= breakfactor * vel.Y; + vec.Z -= breakfactor * vel.Z; + } + if (vec.IsFinite()) { if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index e66580d..5122ebf 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -94,7 +94,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.length = length; req.Normal = direction; req.Origin = position; - req.filter = RayFilterFlags.AllButLand; + req.filter = RayFilterFlags.AllPrims; m_PendingRequests.Enqueue(req); } @@ -115,7 +115,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = DefaultMaxCount; - req.filter = RayFilterFlags.AllButLand; + req.filter = RayFilterFlags.AllPrims; m_PendingRequests.Enqueue(req); } @@ -129,7 +129,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.length = length; req.Normal = direction; req.Origin = position; - req.filter = RayFilterFlags.AllButLand; + req.filter = RayFilterFlags.AllPrims; m_PendingRequests.Enqueue(req); } @@ -143,7 +143,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = DefaultMaxCount; - req.filter = RayFilterFlags.AllButLand; + req.filter = RayFilterFlags.AllPrims; m_PendingRequests.Enqueue(req); } @@ -165,7 +165,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = count; - req.filter = RayFilterFlags.AllButLand; + req.filter = RayFilterFlags.AllPrims; m_PendingRequests.Enqueue(req); } @@ -194,7 +194,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = count; - req.filter = RayFilterFlags.AllButLand; + req.filter = RayFilterFlags.AllPrims; m_PendingRequests.Enqueue(req); } @@ -208,7 +208,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = count; - req.filter = RayFilterFlags.AllButLand; + req.filter = RayFilterFlags.AllPrims; m_PendingRequests.Enqueue(req); } @@ -222,7 +222,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = count; - req.filter = RayFilterFlags.AllButLand; + req.filter = RayFilterFlags.AllPrims; m_PendingRequests.Enqueue(req); } -- cgit v1.1 From 36207b88ffc7801fb15e544e727cb3efaa25d6ea Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 17 Apr 2012 01:00:50 +0100 Subject: ubitODE: bug fix let avatars colide with volume detectors --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 1 + 1 file changed, 1 insertion(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 3185aad..9c1b87b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -117,6 +117,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Default, Collide with Other Geometries, spaces, bodies and characters. private CollisionCategories m_collisionFlags = (CollisionCategories.Character | CollisionCategories.Geom + | CollisionCategories.VolumeDtc ); // we do land collisions not ode | CollisionCategories.Land); public IntPtr Body = IntPtr.Zero; -- cgit v1.1 From d7e24542818ed3edfa57ce748ad2c0bad8f694f4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 17 Apr 2012 14:24:13 +0100 Subject: ubitODE: - made avatar/ground collision pid servo a bit softer since seems a bit unstable with small avas in AVI even if fine on my testsite - Removed reading of PID parameters from config files since that only serves to mess things up and adds more unknowns --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 4 ++-- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 9c1b87b..1c8de56 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -851,10 +851,10 @@ namespace OpenSim.Region.Physics.OdePlugin float depth = terrainheight - chrminZ; if (!flying) { - vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50; + vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 30; } else - vec.Z = depth * PID_P * 50; + vec.Z = depth * PID_P * 30; /* Vector3 vtmp; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 7632e25..837eae3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -455,15 +455,15 @@ namespace OpenSim.Region.Physics.OdePlugin geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); - +/* bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD); bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG); - +*/ forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim); meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD); MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); - +/* if (Environment.OSVersion.Platform == PlatformID.Unix) { avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD); @@ -471,10 +471,11 @@ namespace OpenSim.Region.Physics.OdePlugin } else { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD); avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP); } - +*/ physics_logging = physicsconfig.GetBoolean("physics_logging", false); physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); -- cgit v1.1 From 9132c9e49963c656e303815e5cb9e0c4341f0821 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 17 Apr 2012 15:50:14 +0100 Subject: ubitODE: - character managed ode was only getting position etc from unmanaged at heartbeat rate like core ode. Now do it at ODE rate in move(..). UpdatePositionAndVelocity() called once per heartbeat is now empty. --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 162 ++++++--------------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 5 +- 2 files changed, 48 insertions(+), 119 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 1c8de56..b9ec6b5 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -771,16 +771,10 @@ namespace OpenSim.Region.Physics.OdePlugin /// public void Move(float timeStep, List defects) { - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - if (Body == IntPtr.Zero) return; - d.Vector3 dtmp; - d.BodyCopyPosition(Body, out dtmp); + d.Vector3 dtmp = d.BodyGetPosition(Body); Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); // the Amotor still lets avatar rotation to drift during colisions @@ -797,22 +791,43 @@ namespace OpenSim.Region.Physics.OdePlugin { _zeroPosition = localpos; } - //PidStatus = true; - if (!localpos.IsFinite()) { - m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); defects.Add(this); // _parent_scene.RemoveCharacter(this); // destroy avatar capsule and related ODE data AvatarGeomAndBodyDestroy(); - return; } + // check outbounds forcing to be in world + bool fixbody = false; + if (localpos.X < 0.0f) + { + fixbody = true; + localpos.X = 0.1f; + } + else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f) + { + fixbody = true; + localpos.X = _parent_scene.WorldExtents.X - 0.1f; + } + if (localpos.Y < 0.0f) + { + fixbody = true; + localpos.Y = 0.1f; + } + else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1) + { + fixbody = true; + localpos.Y = _parent_scene.WorldExtents.Y - 0.1f; + } + if (fixbody) + d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); + Vector3 vec = Vector3.Zero; dtmp = d.BodyGetLinearVel(Body); Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); @@ -820,16 +835,12 @@ namespace OpenSim.Region.Physics.OdePlugin float movementdivisor = 1f; //Ubit change divisions into multiplications below if (!m_alwaysRun) - { movementdivisor = 1 / walkDivisor; - } else - { movementdivisor = 1 / runDivisor; - } + //****************************************** // colide with land - d.AABB aabb; d.GeomGetAABB(Shell, out aabb); float chrminZ = aabb.MinZ; @@ -856,26 +867,6 @@ namespace OpenSim.Region.Physics.OdePlugin else vec.Z = depth * PID_P * 30; - /* - Vector3 vtmp; - vtmp.X = _target_velocity.X * timeStep; - vtmp.Y = _target_velocity.Y * timeStep; - // fake and avoid squares - float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y)); - if (k > 0) - { - posch.X += vtmp.X; - posch.Y += vtmp.Y; - terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); - k = 1 + Math.Abs(terrainheight) / k; - movementdivisor /= k; - - if (k < 1) - k = 1; - } - */ - - if (depth < 0.1f) { m_iscolliding = true; @@ -901,6 +892,7 @@ namespace OpenSim.Region.Physics.OdePlugin else m_iscollidingGround = false; + //****************************************** // if velocity is zero, use position control; otherwise, velocity control if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f @@ -1012,97 +1004,31 @@ namespace OpenSim.Region.Physics.OdePlugin // _parent_scene.RemoveCharacter(this); // destroy avatar capsule and related ODE data AvatarGeomAndBodyDestroy(); + return; } + + // update our local ideia of position velocity and aceleration + _position = localpos; + _acceleration = _velocity; // previus velocity + _velocity = vel; + _acceleration = (vel - _acceleration) / timeStep; + } /// - /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. + /// Updates the reported position and velocity. + /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording + /// also outbounds checking + /// copy and outbounds now done in move(..) at ode rate + /// /// public void UpdatePositionAndVelocity() { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (Body == IntPtr.Zero) - return; + return; - d.Vector3 vec; - try - { - d.BodyCopyPosition(Body, out vec); - } - catch (NullReferenceException) - { - bad = true; - _parent_scene.BadCharacter(this); - vec = new d.Vector3(_position.X, _position.Y, _position.Z); - base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem! - m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid); - } - - _position.X = vec.X; - _position.Y = vec.Y; - _position.Z = vec.Z; - - bool fixbody = false; - - if (_position.X < 0.0f) - { - fixbody = true; - _position.X = 0.1f; - } - else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f) - { - fixbody = true; - _position.X = (int)_parent_scene.WorldExtents.X - 0.1f; - } - - if (_position.Y < 0.0f) - { - fixbody = true; - _position.Y = 0.1f; - } - else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1) - { - fixbody = true; - _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f; - } +// if (Body == IntPtr.Zero) +// return; - if (fixbody) - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - - // Did we move last? = zeroflag - // This helps keep us from sliding all over -/* - if (_zeroFlag) - { - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; - - // Did we send out the 'stopped' message? - if (!m_lastUpdateSent) - { - m_lastUpdateSent = true; - base.RequestPhysicsterseUpdate(); - } - } - else - { - m_lastUpdateSent = false; - */ - try - { - vec = d.BodyGetLinearVel(Body); - } - catch (NullReferenceException) - { - vec.X = _velocity.X; - vec.Y = _velocity.Y; - vec.Z = _velocity.Z; - } - _velocity.X = (vec.X); - _velocity.Y = (vec.Y); - _velocity.Z = (vec.Z); - // } } /// diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 837eae3..9ca2d3f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1861,6 +1861,9 @@ namespace OpenSim.Region.Physics.OdePlugin statstart = Util.EnvironmentTickCount(); +/* +// now included in characters move() and done at ode rate +// maybe be needed later if we need to do any extra work at hearbeat rate lock (_characters) { foreach (OdeCharacter actor in _characters) @@ -1874,7 +1877,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } } - +*/ lock (_badCharacter) { if (_badCharacter.Count > 0) -- cgit v1.1 From 9464fcebcd9f0a9b1846113e8aff56ea3f49ace2 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 17 Apr 2012 16:49:08 +0100 Subject: ubitODE: prims - update managed dinamic parameters from unmanaged at ODE rate and not heartbeat. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 124 +++++++++--------------- 1 file changed, 46 insertions(+), 78 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 32c4722..5467b9f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -3228,17 +3228,13 @@ namespace OpenSim.Region.Physics.OdePlugin { if (!childPrim && m_isphysical && Body != IntPtr.Zero && !m_disabled && !m_isSelected && !m_building && !m_outbounds) - // !m_disabled && !m_isSelected && !m_building && !m_outbounds) { - // if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 - if (d.BodyIsEnabled(Body)) { float timestep = _parent_scene.ODE_STEPSIZE; // check outside region - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator if (lpos.Z < -100 || lpos.Z > 100000f) { @@ -3321,12 +3317,9 @@ namespace OpenSim.Region.Physics.OdePlugin { // 'VEHICLES' are dealt with in ODEDynamics.cs m_vehicle.Step(); - return; } - else { - float fx = 0; float fy = 0; float fz = 0; @@ -3512,10 +3505,39 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); } - } + + // update our ideia of velocities and acelerations + d.Quaternion ori; + d.Vector3 dtmpu; + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + d.GeomCopyQuaternion(prim_geom, out ori); + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + + _acceleration = _velocity; + + dtmpu = d.BodyGetLinearVel(Body); + _velocity.X = dtmpu.X; + _velocity.Y = dtmpu.Y; + _velocity.Z = dtmpu.Z; + + float invts = 1 / timestep; + _acceleration = (_velocity - _acceleration) * invts; + + dtmpu = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmpu.X; + m_rotationalVelocity.Y = dtmpu.Y; + m_rotationalVelocity.Z = dtmpu.Z; } - else // body disabled + + else // body disabled/sleeping { // let vehicles sleep if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) @@ -3546,36 +3568,23 @@ namespace OpenSim.Region.Physics.OdePlugin { if (Body != IntPtr.Zero) { - Vector3 pv = Vector3.Zero; bool lastZeroFlag = _zeroFlag; - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - - - d.Quaternion ori; - d.GeomCopyQuaternion(prim_geom, out ori); - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 rotvel = d.BodyGetAngularVel(Body); - - if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) - && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) - && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) - && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) - && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) - && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) + if ((Math.Abs(m_lastposition.X - _position.X) < 0.01) + && (Math.Abs(m_lastposition.Y - _position.Y) < 0.01) + && (Math.Abs(m_lastposition.Z - _position.Z) < 0.01) + && (Math.Abs(m_lastorientation.X - _orientation.X) < 0.0001) + && (Math.Abs(m_lastorientation.Y - _orientation.Y) < 0.0001) + && (Math.Abs(m_lastorientation.Z - _orientation.Z) < 0.0001) ) { _zeroFlag = true; - //Console.WriteLine("ZFT 2"); m_throttleUpdates = false; } else { - //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); _zeroFlag = false; m_lastUpdateSent = false; - //m_throttleUpdates = false; } if (_zeroFlag) @@ -3583,22 +3592,14 @@ namespace OpenSim.Region.Physics.OdePlugin m_lastposition = _position; m_lastorientation = _orientation; - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; if (!m_lastUpdateSent) { m_throttleUpdates = false; throttleCounter = 0; - m_rotationalVelocity = pv; base.RequestPhysicsterseUpdate(); @@ -3612,39 +3613,12 @@ namespace OpenSim.Region.Physics.OdePlugin base.RequestPhysicsterseUpdate(); } - m_lastVelocity = _velocity; - - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - - _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - { - m_rotationalVelocity = pv; - } - else - { - m_rotationalVelocity.X = rotvel.X; - m_rotationalVelocity.Y = rotvel.Y; - m_rotationalVelocity.Z = rotvel.Z; - } - m_lastUpdateSent = false; if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) { m_lastposition = _position; m_lastorientation = _orientation; + m_lastVelocity = _velocity; base.RequestPhysicsterseUpdate(); } else @@ -3656,17 +3630,11 @@ namespace OpenSim.Region.Physics.OdePlugin else if (!m_lastUpdateSent || !_zeroFlag) { // Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + m_lastVelocity = Vector3.Zero; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; _zeroFlag = true; if (!m_lastUpdateSent) -- cgit v1.1 From 7f420692958e05f9e2277911627b730c3066ae70 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 18 Apr 2012 03:02:28 +0100 Subject: ubitODE - retouch character PIDs --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b9ec6b5..b28bc4a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -683,7 +683,7 @@ namespace OpenSim.Region.Physics.OdePlugin PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; PID_P /= 50 * 80; PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; - + Body = d.BodyCreate(_parent_scene.world); d.BodySetAutoDisableFlag(Body, false); @@ -862,10 +862,10 @@ namespace OpenSim.Region.Physics.OdePlugin float depth = terrainheight - chrminZ; if (!flying) { - vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 30; + vec.Z = -vel.Z * PID_D * 3f + depth * PID_P * 60; } else - vec.Z = depth * PID_P * 30; + vec.Z = depth * PID_P * 60; if (depth < 0.1f) { @@ -1171,7 +1171,7 @@ namespace OpenSim.Region.Physics.OdePlugin CAPSULE_LENGTH = caplen; AvatarGeomAndBodyCreation(_position.X, _position.Y, - _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); + _position.Z + (CAPSULE_LENGTH - prevCapsule) * 0.5f); Velocity = Vector3.Zero; -- cgit v1.1 From 08714a0d3826acee9f0e0bfa278efe97206dce18 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 18 Apr 2012 03:59:38 +0100 Subject: ubitODE still retouching character pid --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b28bc4a..ec4be58 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -862,12 +862,12 @@ namespace OpenSim.Region.Physics.OdePlugin float depth = terrainheight - chrminZ; if (!flying) { - vec.Z = -vel.Z * PID_D * 3f + depth * PID_P * 60; + vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 60; } else vec.Z = depth * PID_P * 60; - if (depth < 0.1f) + if (depth < 0.2f) { m_iscolliding = true; m_colliderfilter = 2; -- cgit v1.1 From ec6347f987cc1e42761ff9bd4832da4f999401f0 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 20 Apr 2012 03:17:36 +0100 Subject: ubitODE - again avatar/terrain collision. Reduce new viewers interpolators efects reporting null velocity and aceleration when stopped near the right position, where they can still have instantanius large values that can get magnified by interpolators, specially using diferent timing estimation. --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index ec4be58..f4aa231 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -862,12 +862,12 @@ namespace OpenSim.Region.Physics.OdePlugin float depth = terrainheight - chrminZ; if (!flying) { - vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 60; + vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50; } else - vec.Z = depth * PID_P * 60; + vec.Z = depth * PID_P * 50; - if (depth < 0.2f) + if (depth < 0.1f) { m_iscolliding = true; m_colliderfilter = 2; @@ -1009,9 +1009,17 @@ namespace OpenSim.Region.Physics.OdePlugin // update our local ideia of position velocity and aceleration _position = localpos; - _acceleration = _velocity; // previus velocity - _velocity = vel; - _acceleration = (vel - _acceleration) / timeStep; + if (_zeroFlag) + { + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + } + else + { + _acceleration = _velocity; // previus velocity + _velocity = vel; + _acceleration = (vel - _acceleration) / timeStep; + } } -- cgit v1.1 From 2c7f03592571292ea3a563addb1c8fa6af1f6c59 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 20 Apr 2012 04:49:23 +0100 Subject: ubitODE: - Change triangles used in terrain height estimation --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 39 +++++++++++++++++------- 1 file changed, 28 insertions(+), 11 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 9ca2d3f..72ac605 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2007,7 +2007,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else // out world use external height { - ix = regsize - 1; + ix = regsize - 2; dx = 0; } if (y < regsize - 1) @@ -2017,7 +2017,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - iy = regsize - 1; + iy = regsize - 2; dy = 0; } } @@ -2034,7 +2034,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else // out world use external height { - iy = regsize - 1; + iy = regsize - 2; dy = 0; } if (y < regsize - 1) @@ -2044,7 +2044,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - ix = regsize - 1; + ix = regsize - 2; dx = 0; } } @@ -2057,18 +2057,35 @@ namespace OpenSim.Region.Physics.OdePlugin iy += ix; // all indexes have iy + ix float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + /* + if ((dx + dy) <= 1.0f) + { + h0 = ((float)heights[iy]); // 0,0 vertice + h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + } + else + { + h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice + h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 + h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + } + */ + h0 = ((float)heights[iy]); // 0,0 vertice - if ((dx + dy) <= 1.0f) + if ((dy > dx)) { - h0 = ((float)heights[iy]); // 0,0 vertice - h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 - h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + iy += regsize; + h2 = (float)heights[iy]; // 0,1 vertice + h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 + h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 } else { - h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice - h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 - h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + iy++; + h2 = (float)heights[iy]; // vertice 1,0 + h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 } return h0 + h1 + h2; -- cgit v1.1 From 190e7a43349d1adf28d52b0e7b74fc91a76fbbdb Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 21 Apr 2012 05:16:54 +0100 Subject: ubitODE: - don't try to hover underground unless volumedetector (that doesn't colide with it) --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 140 ++++++++++++++++-------- 1 file changed, 93 insertions(+), 47 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 5467b9f..49766f8 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -68,9 +68,17 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_fakeisphysical; private bool m_isphantom; private bool m_fakeisphantom; + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively protected bool m_building; protected bool m_forcePosOrRotation; + private bool m_iscolliding; + + internal bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + internal bool m_outbounds; private Quaternion m_lastorientation = new Quaternion(); private Quaternion _orientation; @@ -153,14 +161,6 @@ namespace OpenSim.Region.Physics.OdePlugin private List childrenPrim = new List(); - private bool m_iscolliding; - - public bool m_isSelected; - private bool m_delaySelect; - private bool m_lastdoneSelected; - public bool m_outbounds; - - internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively private bool m_throttleUpdates; private int throttleCounter; @@ -223,9 +223,12 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool IsVolumeDtc { - set { return; } - get { return m_isVolumeDetect; } - + get { return m_fakeisVolumeDetect; } + set + { + m_fakeisVolumeDetect = value; + AddChange(changes.VolumeDtc, value); + } } @@ -234,10 +237,7 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_fakeisphantom; } set { - m_fakeisphantom = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - + m_fakeisphantom = value; AddChange(changes.Phantom, value); } } @@ -427,7 +427,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SetVolumeDetect(int param) { - AddChange(changes.VolumeDtc, (param != 0)); + m_fakeisVolumeDetect = (param != 0); + AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); } public override Vector3 GeometricCenter @@ -958,6 +959,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_fakeisphysical = m_isphysical; m_isVolumeDetect = false; + m_fakeisVolumeDetect = false; m_force = Vector3.Zero; @@ -1066,7 +1068,7 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_collisionCategories = CollisionCategories.Selected; prm.m_collisionFlags = 0; } - else if (prm.IsVolumeDtc) + else if (prm.m_isVolumeDetect) { prm.m_collisionCategories = CollisionCategories.VolumeDtc; if (m_isphysical) @@ -1445,14 +1447,14 @@ namespace OpenSim.Region.Physics.OdePlugin hasOOBoffsetFromMesh = false; CalcPrimBodyData(); } - +/* private void ChildSetGeom(OdePrim odePrim) { // well.. DestroyBody(); MakeBody(); } - +*/ //sets non physical prim m_targetSpace to right space in spaces grid for static prims // should only be called for non physical prims unless they are becoming non physical private void SetInStaticSpace(OdePrim prim) @@ -2650,6 +2652,31 @@ namespace OpenSim.Region.Physics.OdePlugin ApplyCollisionCatFlags(); } +/* not in use + internal void ChildSelectedChange(bool childSelect) + { + if(childPrim) + return; + + if (childSelect == m_isSelected) + return; + + if (childSelect) + { + DoSelectedStatus(true); + } + + else + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.m_isSelected) + return; + } + DoSelectedStatus(false); + } + } +*/ private void changeSelectedStatus(bool newval) { if (m_lastdoneSelected == newval) @@ -2669,6 +2696,12 @@ namespace OpenSim.Region.Physics.OdePlugin private void DoSelectedStatus(bool newval) { + if (m_isSelected == newval) + { + resetCollisionAccounting(); + return; + } + m_isSelected = newval; Stop(); @@ -2706,6 +2739,9 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_delaySelect = false; } } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(true); + if (prim_geom != null) { @@ -2741,8 +2777,13 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - if (!childPrim && Body != IntPtr.Zero && !m_disabled) - d.BodyEnable(Body); + if (!childPrim) + { + if (Body != IntPtr.Zero && !m_disabled) + d.BodyEnable(Body); + } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(false); UpdateCollisionCatFlags(); ApplyCollisionCatFlags(); @@ -3145,6 +3186,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeVolumedetetion(bool newVolDtc) { m_isVolumeDetect = newVolDtc; + m_fakeisVolumeDetect = newVolDtc; UpdateCollisionCatFlags(); ApplyCollisionCatFlags(); } @@ -3370,7 +3412,7 @@ namespace OpenSim.Region.Physics.OdePlugin //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); + // d.BodyAddForce(Body, 0, 0, fz); return; } else @@ -3419,14 +3461,17 @@ namespace OpenSim.Region.Physics.OdePlugin { case PIDHoverType.Ground: m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; case PIDHoverType.GroundAndWater: m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); m_waterHeight = _parent_scene.GetWaterLevel(); if (m_groundHeight > m_waterHeight) { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + if (m_PIDHoverHeight > 0 || m_isVolumeDetect) + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + else + m_targetHoverHeight = m_groundHeight; } else { @@ -3436,34 +3481,35 @@ namespace OpenSim.Region.Physics.OdePlugin } // end switch (m_PIDHoverType) + // don't go underground unless volumedetector + + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + { + fz = (m_targetHoverHeight - pos.Z) * (PID_G - m_PIDHoverTau) * timestep; - _target_velocity = - new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control + // if velocity is zero, use position control; otherwise, velocity control - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' + if (Math.Abs(fz) < 0.1f) + { + // keep track of where we stopped. No more slippin' & slidin' - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - // ? d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; - // We're flying and colliding with something - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + // We're flying and colliding with something + fz = ((fz - vel.Z) * (PID_D)); + } } } else -- cgit v1.1 From e0f81e24000df3a969cd313d008194d8226272ff Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 23 Apr 2012 01:47:11 +0100 Subject: ubitODE - several changes... --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 + OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 676 ++++++++++----------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 131 ++-- 3 files changed, 379 insertions(+), 431 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index f4aa231..6ffcb9e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -686,6 +686,9 @@ namespace OpenSim.Region.Physics.OdePlugin Body = d.BodyCreate(_parent_scene.world); + _zeroFlag = false; + m_pidControllerActive = true; + d.BodySetAutoDisableFlag(Body, false); d.BodySetPosition(Body, npositionX, npositionY, npositionZ); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 49766f8..7c0bbef 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -98,10 +98,12 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_forceacc; private Vector3 m_angularForceacc; + private float m_invTimeStep = 50.0f; + private float m_timeStep = .02f; + + private Vector3 m_PIDTarget; private float m_PIDTau; - private float PID_D = 35f; - private float PID_G = 25f; private bool m_usePID; // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), @@ -632,7 +634,6 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 pv = Vector3.Zero; if (_zeroFlag) return pv; - m_lastUpdateSent = false; if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) return pv; @@ -686,12 +687,50 @@ namespace OpenSim.Region.Physics.OdePlugin } public override bool PIDActive { set { m_usePID = value; } } - public override float PIDTau { set { m_PIDTau = value; } } + public override float PIDTau + { + set + { + if (value <= 0) + m_PIDTau = 0; + else + { + float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE); + if (value < mint) + m_PIDTau = mint; + else + m_PIDTau = value; + } + } + } - public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override float PIDHoverHeight + { + set + { + m_PIDHoverHeight = value; + if (value == 0) + m_useHoverPID = false; + } + } public override bool PIDHoverActive { set { m_useHoverPID = value; } } public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } - public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + public override float PIDHoverTau + { + set + { + if (value <= 0) + m_PIDHoverTau = 0; + else + { + float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE); + if (value < mint) + m_PIDHoverTau = mint; + else + m_PIDHoverTau = value; + } + } + } public override Quaternion APIDTarget { set { return; } } @@ -912,8 +951,9 @@ namespace OpenSim.Region.Physics.OdePlugin _position = pos; givefakepos = 0; - PID_D = parent_scene.bodyPIDD; - PID_G = parent_scene.bodyPIDG; + m_timeStep = parent_scene.ODE_STEPSIZE; + m_invTimeStep = 1f / m_timeStep; + m_density = parent_scene.geomDefaultDensity; // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; body_autodisable_frames = parent_scene.bodyFramesAutoDisable; @@ -2696,12 +2736,6 @@ namespace OpenSim.Region.Physics.OdePlugin private void DoSelectedStatus(bool newval) { - if (m_isSelected == newval) - { - resetCollisionAccounting(); - return; - } - m_isSelected = newval; Stop(); @@ -2970,7 +3004,6 @@ namespace OpenSim.Region.Physics.OdePlugin givefakeori--; if (givefakeori < 0) givefakeori = 0; - resetCollisionAccounting(); } @@ -3125,9 +3158,10 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(Body); } - private void changeAddForce(Vector3 force) + + private void changeAddImpulse(Vector3 impulse) { - m_forceacc += force; + m_forceacc += impulse * m_invTimeStep; if (!m_isSelected) { lock (this) @@ -3146,9 +3180,10 @@ namespace OpenSim.Region.Physics.OdePlugin } } - private void changeAddAngularForce(Vector3 aforce) + // actually angular impulse + private void changeAddAngularImpulse(Vector3 aimpulse) { - m_angularForceacc += aforce; + m_angularForceacc += aimpulse * m_invTimeStep; if (!m_isSelected) { lock (this) @@ -3271,330 +3306,246 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim && m_isphysical && Body != IntPtr.Zero && !m_disabled && !m_isSelected && !m_building && !m_outbounds) { - if (d.BodyIsEnabled(Body)) + if (!d.BodyIsEnabled(Body)) { - float timestep = _parent_scene.ODE_STEPSIZE; - - // check outside region - d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - - if (lpos.Z < -100 || lpos.Z > 100000f) - { - m_outbounds = true; - - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); + // let vehicles sleep + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) return; - } - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } - - if (m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; - - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - base.RequestPhysicsterseUpdate(); + if (++bodydisablecontrol < 20) return; - } - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(); - } - else - { - float fx = 0; - float fy = 0; - float fz = 0; - - float m_mass = _mass; - - // fz = 0f; - //m_log.Info(m_collisionFlags.ToString()); - if (m_usePID) - { + bodydisablecontrol = 0; + d.BodyEnable(Body); + } - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + // check outside region + d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; - // if velocity is zero, use position control; otherwise, velocity control + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - // d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + base.RequestPhysicsterseUpdate(); - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + throttleCounter = 0; + _zeroFlag = true; - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } - fz = ((_target_velocity.Z - vel.Z) * (PID_D)); - } - } // end if (m_usePID) + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID) - { - //Console.WriteLine("Hover " + Name); + if (m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; - // If we're using the PID controller, then we have no gravity + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; - // no lock; for now it's only called from within Simulate() + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + return; + } - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } + float fx = 0; + float fy = 0; + float fz = 0; - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); + float m_mass = _mass; - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + if (m_usePID && m_PIDTau > 0) + { + // for now position error + _target_velocity = + new Vector3( + (m_PIDTarget.X - lpos.X), + (m_PIDTarget.Y - lpos.Y), + (m_PIDTarget.Z - lpos.Z) + ); - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - if (m_PIDHoverHeight > 0 || m_isVolumeDetect) - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - else - m_targetHoverHeight = m_groundHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) + { + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + return; + } + else + { + _zeroFlag = false; - } // end switch (m_PIDHoverType) + float tmp = 1 / m_PIDTau; + _target_velocity *= tmp; - // don't go underground unless volumedetector - - if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) - { - fz = (m_targetHoverHeight - pos.Z) * (PID_G - m_PIDHoverTau) * timestep; + // apply limits + tmp = _target_velocity.Length(); + if (tmp > 50.0f) + { + tmp = 50 / tmp; + _target_velocity *= tmp; + } + else if (tmp < 0.05f) + { + tmp = 0.05f / tmp; + _target_velocity *= tmp; + } - // if velocity is zero, use position control; otherwise, velocity control + d.Vector3 vel = d.BodyGetLinearVel(Body); + fx = (_target_velocity.X - vel.X) * m_invTimeStep; + fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; + fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; + } + } // end if (m_usePID) - if (Math.Abs(fz) < 0.1f) - { - // keep track of where we stopped. No more slippin' & slidin' + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) + { - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - // ? d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); - // We're flying and colliding with something - fz = ((fz - vel.Z) * (PID_D)); - } - } - } - else - { - float b = (1.0f - m_buoyancy); - fx = _parent_scene.gravityx * b; - fy = _parent_scene.gravityy * b; - fz = _parent_scene.gravityz * b; - } + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; + case PIDHoverType.GroundAndWater: + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + else + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + break; + } // end switch (m_PIDHoverType) - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; + // don't go underground unless volumedetector - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + { + d.Vector3 vel = d.BodyGetLinearVel(Body); - m_forceacc = Vector3.Zero; + fz = (m_targetHoverHeight - lpos.Z); - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) + // if error is zero, use position control; otherwise, velocity control + if (Math.Abs(fz) < 0.01f) { - d.BodyAddForce(Body, fx, fy, fz); - //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + return; } + else + { + _zeroFlag = false; + fz /= m_PIDHoverTau; - Vector3 trq; + float tmp = Math.Abs(fz); + if (tmp > 50) + fz = 50 * Math.Sign(fz); + else if (tmp < 0.1) + fz = 0.1f * Math.Sign(fz); - trq = _torque; - trq += m_angularForceacc; - m_angularForceacc = Vector3.Zero; - if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) - { - d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + fz = ((fz - vel.Z) * m_invTimeStep); } } + } + else + { + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } - // update our ideia of velocities and acelerations - d.Quaternion ori; - d.Vector3 dtmpu; - - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - - d.GeomCopyQuaternion(prim_geom, out ori); - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; - _acceleration = _velocity; + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; - dtmpu = d.BodyGetLinearVel(Body); - _velocity.X = dtmpu.X; - _velocity.Y = dtmpu.Y; - _velocity.Z = dtmpu.Z; + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; - float invts = 1 / timestep; - _acceleration = (_velocity - _acceleration) * invts; + m_forceacc = Vector3.Zero; - dtmpu = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmpu.X; - m_rotationalVelocity.Y = dtmpu.Y; - m_rotationalVelocity.Z = dtmpu.Z; + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); } - else // body disabled/sleeping - { - // let vehicles sleep - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - return; + Vector3 trq; - if (++bodydisablecontrol < 20) - return; - - bodydisablecontrol = 0; - d.BodyEnable(Body); - return; + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); } } else @@ -3606,92 +3557,113 @@ namespace OpenSim.Region.Physics.OdePlugin } } - - public void UpdatePositionAndVelocity(float simulatedtime) + public void UpdatePositionAndVelocity() { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null && !m_disabled && !m_building && !m_outbounds) + if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) { - if (Body != IntPtr.Zero) + if (d.BodyIsEnabled(Body) || !_zeroFlag) { bool lastZeroFlag = _zeroFlag; - if ((Math.Abs(m_lastposition.X - _position.X) < 0.01) - && (Math.Abs(m_lastposition.Y - _position.Y) < 0.01) - && (Math.Abs(m_lastposition.Z - _position.Z) < 0.01) - && (Math.Abs(m_lastorientation.X - _orientation.X) < 0.0001) - && (Math.Abs(m_lastorientation.Y - _orientation.Y) < 0.0001) - && (Math.Abs(m_lastorientation.Z - _orientation.Z) < 0.0001) + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + + // decide if moving + // use positions since this are integrated quantities + // tolerance values depende a lot on simulation noise... + // use simple math.abs since we dont need to be exact + + if ( + (Math.Abs(_position.X - lpos.X) < 0.001f) + && (Math.Abs(_position.Y - lpos.Y) < 0.001f) + && (Math.Abs(_position.Z - lpos.Z) < 0.001f) + && (Math.Abs(_orientation.X - ori.X) < 0.0001f) + && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f) + && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W ) { _zeroFlag = true; - m_throttleUpdates = false; } else - { _zeroFlag = false; - m_lastUpdateSent = false; - } - if (_zeroFlag) + // update velocities and aceleration + if (!(_zeroFlag && lastZeroFlag)) { - m_lastposition = _position; - m_lastorientation = _orientation; + d.Vector3 vel = d.BodyGetLinearVel(Body); - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; + _acceleration = _velocity; - if (!m_lastUpdateSent) + if ((Math.Abs(vel.X) < 0.001f) && + (Math.Abs(vel.Y) < 0.001f) && + (Math.Abs(vel.Z) < 0.001f)) { - m_throttleUpdates = false; - throttleCounter = 0; - - base.RequestPhysicsterseUpdate(); - - m_lastUpdateSent = true; + _velocity = Vector3.Zero; + float t = -m_invTimeStep; + _acceleration = _acceleration * t; } - } - else - { - if (lastZeroFlag != _zeroFlag) + else { - base.RequestPhysicsterseUpdate(); + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + _acceleration = (_velocity - _acceleration) * m_invTimeStep; } - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + if ((Math.Abs(_acceleration.X) < 0.01f) && + (Math.Abs(_acceleration.Y) < 0.01f) && + (Math.Abs(_acceleration.Z) < 0.01f)) { - m_lastposition = _position; - m_lastorientation = _orientation; - m_lastVelocity = _velocity; - base.RequestPhysicsterseUpdate(); + _acceleration = Vector3.Zero; + } + + if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && + (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && + (Math.Abs(_orientation.Z - ori.Z) < 0.0001) + ) + { + m_rotationalVelocity = Vector3.Zero; } else { - throttleCounter++; + vel = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = vel.X; + m_rotationalVelocity.Y = vel.Y; + m_rotationalVelocity.Z = vel.Z; } } - } - else if (!m_lastUpdateSent || !_zeroFlag) - { - // Not a body.. so Make sure the client isn't interpolating - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - m_lastVelocity = Vector3.Zero; - - _zeroFlag = true; - if (!m_lastUpdateSent) + if (_zeroFlag) { - m_throttleUpdates = false; - throttleCounter = 0; - - base.RequestPhysicsterseUpdate(); + if (lastZeroFlag) + { + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + } - m_lastUpdateSent = true; + if (!m_lastUpdateSent) + { + base.RequestPhysicsterseUpdate(); + if (lastZeroFlag) + m_lastUpdateSent = true; + } + return; } + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + base.RequestPhysicsterseUpdate(); + m_lastUpdateSent = false; } } } @@ -3849,11 +3821,11 @@ namespace OpenSim.Region.Physics.OdePlugin break; case changes.AddForce: - changeAddForce((Vector3)arg); + changeAddImpulse((Vector3)arg); break; case changes.AddAngForce: - changeAddAngularForce((Vector3)arg); + changeAddAngularImpulse((Vector3)arg); break; case changes.AngLock: diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 72ac605..1d9fa93 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -521,7 +521,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldSetAngularDamping(world, 0.001f); d.WorldSetAngularDampingThreshold(world, 0f); d.WorldSetLinearDampingThreshold(world, 0f); - d.WorldSetMaxAngularSpeed(world, 256f); + d.WorldSetMaxAngularSpeed(world, 100f); d.WorldSetCFM(world,1e-6f); // a bit harder than default //d.WorldSetCFM(world, 1e-4f); // a bit harder than default @@ -1685,17 +1685,6 @@ namespace OpenSim.Region.Physics.OdePlugin /// public override float Simulate(float timeStep) { - int statstart; - int statchanges = 0; - int statchmove = 0; - int statactmove = 0; - int statray = 0; - int statcol = 0; - int statstep = 0; - int statmovchar = 0; - int statmovprim; - int totjcontact = 0; - // acumulate time so we can reduce error step_time += timeStep; @@ -1738,8 +1727,6 @@ namespace OpenSim.Region.Physics.OdePlugin { try { - statstart = Util.EnvironmentTickCount(); - // clear pointer/counter to contacts to pass into joints m_global_contactcount = 0; @@ -1778,17 +1765,39 @@ namespace OpenSim.Region.Physics.OdePlugin } - statchanges += Util.EnvironmentTickCountSubtract(statstart); + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(ODE_STEPSIZE, defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + } + } + + // Move other active objects + lock (_activegroups) + { + foreach (OdePrim aprim in _activegroups) + { + aprim.Move(); + } + } - statactmove += Util.EnvironmentTickCountSubtract(statstart); //if ((framecount % m_randomizeWater) == 0) // randomizeWater(waterlevel); m_rayCastManager.ProcessQueuedRequests(); - statray += Util.EnvironmentTickCountSubtract(statstart); collision_optimized(); - statcol += Util.EnvironmentTickCountSubtract(statstart); lock (_collisionEventPrim) { @@ -1813,38 +1822,39 @@ namespace OpenSim.Region.Physics.OdePlugin } } + // do a ode simulation step d.WorldQuickStep(world, ODE_STEPSIZE); - statstep += Util.EnvironmentTickCountSubtract(statstart); + d.JointGroupEmpty(contactgroup); + + // update managed ideia of physical data and do updates to core + /* + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); + + actor.UpdatePositionAndVelocity(); + } + } + } + */ - // Move characters - lock (_characters) + lock (_activegroups) { - List defects = new List(); - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - actor.Move(ODE_STEPSIZE, defects); - } - if (defects.Count != 0) { - foreach (OdeCharacter defect in defects) + foreach (OdePrim actor in _activegroups) { - RemoveCharacter(defect); + if (actor.IsPhysical) + { + actor.UpdatePositionAndVelocity(); + } } } } - statchmove += Util.EnvironmentTickCountSubtract(statstart); - - // Move other active objects - lock (_activegroups) - { - foreach (OdePrim aprim in _activegroups) - { - aprim.Move(); - } - } - - //ode.dunlock(world); } catch (Exception e) { @@ -1852,32 +1862,11 @@ namespace OpenSim.Region.Physics.OdePlugin // ode.dunlock(world); } - d.JointGroupEmpty(contactgroup); - totjcontact += m_global_contactcount; step_time -= ODE_STEPSIZE; nodeframes++; } - statstart = Util.EnvironmentTickCount(); - -/* -// now included in characters move() and done at ode rate -// maybe be needed later if we need to do any extra work at hearbeat rate - lock (_characters) - { - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - { - if (actor.bad) - m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); - - actor.UpdatePositionAndVelocity(); - } - } - } -*/ lock (_badCharacter) { if (_badCharacter.Count > 0) @@ -1890,22 +1879,6 @@ namespace OpenSim.Region.Physics.OdePlugin _badCharacter.Clear(); } } - statmovchar = Util.EnvironmentTickCountSubtract(statstart); - - lock (_activegroups) - { - { - foreach (OdePrim actor in _activegroups) - { - if (actor.IsPhysical) - { - actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE); - } - } - } - } - - statmovprim = Util.EnvironmentTickCountSubtract(statstart); int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); -- cgit v1.1 From 5a8fdc8a0b79c14382872571b113b5c5559083c4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 23 Apr 2012 20:16:53 +0100 Subject: ubitODE - do own timing control (as chODE does) until heartbeat does it right --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 8 ++-- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 47 +++++++++++++++--------- 2 files changed, 34 insertions(+), 21 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 7c0bbef..dc6c18d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -695,7 +695,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_PIDTau = 0; else { - float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE); + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); if (value < mint) m_PIDTau = mint; else @@ -723,7 +723,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_PIDHoverTau = 0; else { - float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE); + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); if (value < mint) m_PIDHoverTau = mint; else @@ -801,7 +801,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (force.IsFinite()) { - AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); + AddChange(changes.AddForce, force * m_invTimeStep); } else { @@ -814,7 +814,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (force.IsFinite()) { - AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); + AddChange(changes.AddAngForce, force * m_invTimeStep); } else { diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 1d9fa93..cf74f14 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -189,9 +189,12 @@ namespace OpenSim.Region.Physics.OdePlugin private const uint m_regionHeight = Constants.RegionSize; public float ODE_STEPSIZE = 0.020f; + public float HalfOdeStep = 0.01f; private float metersInSpace = 25.6f; private float m_timeDilation = 1.0f; + DateTime m_lastframe; + public float gravityx = 0f; public float gravityy = 0f; public float gravityz = -9.8f; @@ -485,6 +488,8 @@ namespace OpenSim.Region.Physics.OdePlugin } } + HalfOdeStep = ODE_STEPSIZE * 0.5f; + ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); @@ -564,6 +569,7 @@ namespace OpenSim.Region.Physics.OdePlugin // let this now be real maximum values spaceGridMaxX--; spaceGridMaxY--; + m_lastframe = DateTime.UtcNow; } internal void waitForSpaceUnlock(IntPtr space) @@ -1685,24 +1691,30 @@ namespace OpenSim.Region.Physics.OdePlugin /// public override float Simulate(float timeStep) { + + DateTime now = DateTime.UtcNow; + TimeSpan SinceLastFrame = now - m_lastframe; + m_lastframe = now; + timeStep = (float)SinceLastFrame.TotalSeconds; + // acumulate time so we can reduce error step_time += timeStep; - if (step_time < ODE_STEPSIZE) + if (step_time < HalfOdeStep) return 0; - if (framecount >= int.MaxValue) + if (framecount < 0) framecount = 0; framecount++; - int curphysiteractions = m_physicsiterations; + int curphysiteractions; + // if in trouble reduce step resolution if (step_time >= m_SkipFramesAtms) - { - // if in trouble reduce step resolution - curphysiteractions /= 2; - } + curphysiteractions = m_physicsiterations / 2; + else + curphysiteractions = m_physicsiterations; int nodeframes = 0; @@ -1722,8 +1734,7 @@ namespace OpenSim.Region.Physics.OdePlugin base.TriggerPhysicsBasedRestart(); } - - while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever + while (step_time >= HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever { try { @@ -1905,15 +1916,17 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); } - // think time dilation is not a physics issue alone.. but ok let's fake something - if (step_time < ODE_STEPSIZE) // we did the required loops + // think time dilation as to do with dinamic step size that we dont' have + // even so tell something to world + if (nodeframes < 10) // we did the requested loops m_timeDilation = 1.0f; - else - { // we didn't forget the lost ones and let user know something - m_timeDilation = 1 - step_time / timeStep; - if (m_timeDilation < 0) - m_timeDilation = 0; - step_time = 0; + else if (step_time > 0) + { + m_timeDilation = timeStep / step_time; + if (m_timeDilation > 1) + m_timeDilation = 1; + if (step_time > m_SkipFramesAtms) + step_time = 0; } } -- cgit v1.1 From a4b76a42cc995ab044798e38c418422dcdc0069a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 24 Apr 2012 06:56:34 +0100 Subject: let objects/avas push avas --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 6ffcb9e..8b5b989 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -346,12 +346,11 @@ namespace OpenSim.Region.Physics.OdePlugin m_iscollidingObj = true; // m_iscollidingObj = value; -/* + if (m_iscollidingObj) m_pidControllerActive = false; else m_pidControllerActive = true; - */ } } -- cgit v1.1 From 3da613adf5c803edfcca41f260017b96b1d80fb5 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 24 Apr 2012 23:13:57 +0100 Subject: fix chODE loosing some part positions when doing isPhysical false --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 45 +++++++++++++++++++++---- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 8 ++--- 2 files changed, 42 insertions(+), 11 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 77ea2af..5b743e8 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -461,6 +461,13 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public override bool IsVolumeDtc + { + set { return; } + get { return m_isVolumeDetect; } + + } + public override bool Phantom { get { return m_isphantom; } @@ -598,6 +605,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public override Vector3 CenterOfMass { get { return Vector3.Zero; } @@ -1372,6 +1380,25 @@ namespace OpenSim.Region.Physics.OdePlugin } } + + private void UpdateDataFromGeom() + { + if (prim_geom != IntPtr.Zero) + { + d.Quaternion qtmp; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.W = qtmp.W; + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + + d.Vector3 lpos = d.GeomGetPosition(prim_geom); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + } + } + public void disableBody() { //this kills the body so things like 'mesh' can re-create it. @@ -1400,25 +1427,31 @@ namespace OpenSim.Region.Physics.OdePlugin } } - d.BodyDestroy(Body); + UpdateDataFromGeom(); + lock (childrenPrim) { if (childrenPrim.Count > 0) { foreach (OdePrim prm in childrenPrim) { - if (prm.m_NoColide && prm.prim_geom != IntPtr.Zero) + if (prm.prim_geom != IntPtr.Zero) { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - d.GeomDisable(prm.prim_geom); + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + d.GeomDisable(prm.prim_geom); + + } + prm.UpdateDataFromGeom(); } - _parent_scene.remActivePrim(prm); prm.Body = IntPtr.Zero; } } } + d.BodyDestroy(Body); Body = IntPtr.Zero; } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index dc6c18d..2d587ab 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2467,15 +2467,14 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prim_geom != IntPtr.Zero) { - d.Quaternion qtmp = new d.Quaternion { }; + d.Quaternion qtmp; d.GeomCopyQuaternion(prim_geom, out qtmp); _orientation.W = qtmp.W; _orientation.X = qtmp.X; _orientation.Y = qtmp.Y; _orientation.Z = qtmp.Z; - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); + d.Vector3 lpos = d.GeomGetPosition(prim_geom); _position.X = lpos.X; _position.Y = lpos.Y; _position.Z = lpos.Z; @@ -3565,8 +3564,7 @@ namespace OpenSim.Region.Physics.OdePlugin { bool lastZeroFlag = _zeroFlag; - d.Vector3 lpos; - d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + d.Vector3 lpos = d.GeomGetPosition(prim_geom); d.Quaternion ori; d.GeomCopyQuaternion(prim_geom, out ori); -- cgit v1.1 From a64a9e48de5226da055b0196125c8f576c462a80 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 01:43:27 +0100 Subject: TESTING --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 1 + 1 file changed, 1 insertion(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 8b5b989..f22b0cc 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1210,6 +1210,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); _position = newPos; + m_pidControllerActive = true; } private void changeOrientation(Quaternion newOri) -- cgit v1.1 From 911bc81b00b276792cc43c3b246932186008d343 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 02:13:54 +0100 Subject: testing --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index f22b0cc..7356123 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1275,6 +1275,12 @@ namespace OpenSim.Region.Physics.OdePlugin _velocity = newmomentum; _target_velocity = newmomentum; m_pidControllerActive = true; + m_colliderfilter = 0; + m_colliderObjectfilter = 0; + m_iscolliding = false; + m_iscollidingGround = false; + m_iscollidingObj = false; + if (Body != IntPtr.Zero) d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); } -- cgit v1.1 From 76d9040ed41facca73b8628f381dd8bdec3c9dc2 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 02:54:39 +0100 Subject: testing --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 7356123..4b093ba 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1273,7 +1273,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeMomentum(Vector3 newmomentum) { _velocity = newmomentum; - _target_velocity = newmomentum; + _target_velocity = Vector3.Zero; m_pidControllerActive = true; m_colliderfilter = 0; m_colliderObjectfilter = 0; -- cgit v1.1 From d8f691664a8de15655eb7a3dd0485c21202982e2 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 03:36:49 +0100 Subject: testing --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 129 ++++++++++++--------- 1 file changed, 71 insertions(+), 58 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 4b093ba..2c1197a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -103,6 +103,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_buoyancy = 0f; + private bool m_freemove = false; // private CollisionLocker ode; private string m_name = String.Empty; @@ -687,6 +688,7 @@ namespace OpenSim.Region.Physics.OdePlugin _zeroFlag = false; m_pidControllerActive = true; + m_freemove = false; d.BodySetAutoDisableFlag(Body, false); d.BodySetPosition(Body, npositionX, npositionY, npositionZ); @@ -789,7 +791,7 @@ namespace OpenSim.Region.Physics.OdePlugin qtmp.Z = 0; d.BodySetQuaternion(Body, ref qtmp); - if (m_pidControllerActive == false) + if (m_pidControllerActive == false && !m_freemove) { _zeroPosition = localpos; } @@ -861,6 +863,7 @@ namespace OpenSim.Region.Physics.OdePlugin float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); if (chrminZ < terrainheight) { + m_freemove = false; float depth = terrainheight - chrminZ; if (!flying) { @@ -896,77 +899,86 @@ namespace OpenSim.Region.Physics.OdePlugin //****************************************** - // if velocity is zero, use position control; otherwise, velocity control - if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f - && m_iscolliding) + bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); + + if(!tviszero || !m_iscolliding) + m_freemove = false; + + if (!m_freemove) { - // keep track of where we stopped. No more slippin' & slidin' - if (!_zeroFlag) - { - _zeroFlag = true; - _zeroPosition = localpos; - } - if (m_pidControllerActive) + + // if velocity is zero, use position control; otherwise, velocity control + if (tviszero && m_iscolliding) { - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2); - vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2); - if (flying) + // keep track of where we stopped. No more slippin' & slidin' + if (!_zeroFlag) { - vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + _zeroFlag = true; + _zeroPosition = localpos; } - } - //PidStatus = true; - } - else - { - m_pidControllerActive = true; - _zeroFlag = false; - - if (m_iscolliding) - { - if (!flying) + if (m_pidControllerActive) { - if (_target_velocity.Z > 0.0f) + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2); + vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2); + if (flying) { - // We're colliding with something and we're not flying but we're moving - // This means we're walking or running. JUMPING - vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; } - // We're standing on something - vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D); - vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D); - } - else - { - // We're flying and colliding with something - vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f); - vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f); - vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); } + //PidStatus = true; } - else // ie not colliding + else { - if (flying) //(!m_iscolliding && flying) + m_freemove = false; + m_pidControllerActive = true; + _zeroFlag = false; + + if (m_iscolliding) { - // we're in mid air suspended - vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f); - vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f); - vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + if (!flying) + { + if (_target_velocity.Z > 0.0f) + { + // We're colliding with something and we're not flying but we're moving + // This means we're walking or running. JUMPING + vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; + } + // We're standing on something + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D); + } + else + { + // We're flying and colliding with something + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f); + vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + } } - - else + else // ie not colliding { - // we're not colliding and we're not flying so that means we're falling! - // m_iscolliding includes collisions with the ground. + if (flying) //(!m_iscolliding && flying) + { + // we're in mid air suspended + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f); + vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + } - // d.Vector3 pos = d.BodyGetPosition(Body); - vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f; - vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f; + else + { + // we're not colliding and we're not flying so that means we're falling! + // m_iscolliding includes collisions with the ground. + + // d.Vector3 pos = d.BodyGetPosition(Body); + vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f; + vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f; + } } } } @@ -1274,6 +1286,7 @@ namespace OpenSim.Region.Physics.OdePlugin { _velocity = newmomentum; _target_velocity = Vector3.Zero; + m_freemove = true; m_pidControllerActive = true; m_colliderfilter = 0; m_colliderObjectfilter = 0; -- cgit v1.1 From e974d493c689a7fda925e72de78fddc05cfa5225 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 03:44:15 +0100 Subject: testing --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 2c1197a..1ba40b8 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -901,7 +901,7 @@ namespace OpenSim.Region.Physics.OdePlugin bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); - if(!tviszero || !m_iscolliding) + if(!tviszero || m_iscolliding) m_freemove = false; if (!m_freemove) -- cgit v1.1 From 6edbbdc83ad53f38f1f5ae4a025b03c058827495 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 06:00:50 +0100 Subject: ubitODE fix don't report colisions with a volume detector ( only report to it ) --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 28 ++++++++++++++---------- 1 file changed, 16 insertions(+), 12 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index cf74f14..fa3d33e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1020,12 +1020,16 @@ namespace OpenSim.Region.Physics.OdePlugin private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) { - // obj1LocalID = 0; - //returncollisions = false; obj2LocalID = 0; - //ctype = 0; - //cStartStop = 0; - if (!(p2.SubscribedEvents() || p1.SubscribedEvents())) + bool p1events = p1.SubscribedEvents(); + bool p2events = p2.SubscribedEvents(); + + if (p1 is OdePrim && p1.IsVolumeDtc) + p2events = false; + if (p2 is OdePrim && p2.IsVolumeDtc) + p1events = false; + + if (!(p2events || p1events)) return; switch ((ActorTypes)p1.PhysicsActorType) @@ -1037,7 +1041,7 @@ namespace OpenSim.Region.Physics.OdePlugin case ActorTypes.Agent: cc2 = (OdeCharacter)p2; obj2LocalID = cc2.m_localID; - if (p2.SubscribedEvents()) + if (p2events) cc2.AddCollisionEvent(cc1.m_localID, contact); break; @@ -1046,7 +1050,7 @@ namespace OpenSim.Region.Physics.OdePlugin { cp2 = (OdePrim)p2; obj2LocalID = cp2.m_localID; - if (p2.SubscribedEvents()) + if (p2events) cp2.AddCollisionEvent(cc1.m_localID, contact); } break; @@ -1057,7 +1061,7 @@ namespace OpenSim.Region.Physics.OdePlugin obj2LocalID = 0; break; } - if (p1.SubscribedEvents()) + if (p1events) { contact.SurfaceNormal = -contact.SurfaceNormal; cc1.AddCollisionEvent(obj2LocalID, contact); @@ -1078,7 +1082,7 @@ namespace OpenSim.Region.Physics.OdePlugin { cc2 = (OdeCharacter)p2; obj2LocalID = cc2.m_localID; - if (p2.SubscribedEvents()) + if (p2events) cc2.AddCollisionEvent(cp1.m_localID, contact); } break; @@ -1088,7 +1092,7 @@ namespace OpenSim.Region.Physics.OdePlugin { cp2 = (OdePrim)p2; obj2LocalID = cp2.m_localID; - if (p2.SubscribedEvents()) + if (p2events) cp2.AddCollisionEvent(cp1.m_localID, contact); } break; @@ -1099,7 +1103,7 @@ namespace OpenSim.Region.Physics.OdePlugin obj2LocalID = 0; break; } - if (p1.SubscribedEvents()) + if (p1events) { contact.SurfaceNormal = -contact.SurfaceNormal; cp1.AddCollisionEvent(obj2LocalID, contact); @@ -1734,7 +1738,7 @@ namespace OpenSim.Region.Physics.OdePlugin base.TriggerPhysicsBasedRestart(); } - while (step_time >= HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever + while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever { try { -- cgit v1.1 From 03450dee39460f92d293621bb2fbcf93582397fc Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 08:11:18 +0100 Subject: testing.... --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 119 ++++++++++++++------- 1 file changed, 78 insertions(+), 41 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 1ba40b8..d44c8e6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -109,7 +109,7 @@ namespace OpenSim.Region.Physics.OdePlugin private string m_name = String.Empty; // other filter control int m_colliderfilter = 0; - // int m_colliderGroundfilter = 0; + int m_colliderGroundfilter = 0; int m_colliderObjectfilter = 0; // Default we're a Character @@ -281,7 +281,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_iscolliding = false; else { -// SetPidStatus(false); m_pidControllerActive = true; m_iscolliding = true; } @@ -617,7 +616,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (pushforce) { - AddChange(changes.Force, force * m_density / _parent_scene.ODE_STEPSIZE / 28f); + AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f)); } else { @@ -791,7 +790,7 @@ namespace OpenSim.Region.Physics.OdePlugin qtmp.Z = 0; d.BodySetQuaternion(Body, ref qtmp); - if (m_pidControllerActive == false && !m_freemove) + if (m_pidControllerActive == false) { _zeroPosition = localpos; } @@ -830,11 +829,17 @@ namespace OpenSim.Region.Physics.OdePlugin localpos.Y = _parent_scene.WorldExtents.Y - 0.1f; } if (fixbody) + { + m_freemove = false; d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); + } + + float breakfactor; Vector3 vec = Vector3.Zero; dtmp = d.BodyGetLinearVel(Body); Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + float velLengthSquared = vel.LengthSquared(); float movementdivisor = 1f; //Ubit change divisions into multiplications below @@ -863,7 +868,6 @@ namespace OpenSim.Region.Physics.OdePlugin float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); if (chrminZ < terrainheight) { - m_freemove = false; float depth = terrainheight - chrminZ; if (!flying) { @@ -874,34 +878,46 @@ namespace OpenSim.Region.Physics.OdePlugin if (depth < 0.1f) { - m_iscolliding = true; - m_colliderfilter = 2; - m_iscollidingGround = true; - - ContactPoint contact = new ContactPoint(); - contact.PenetrationDepth = depth; - contact.Position.X = localpos.X; - contact.Position.Y = localpos.Y; - contact.Position.Z = chrminZ; - contact.SurfaceNormal.X = 0f; - contact.SurfaceNormal.Y = 0f; - contact.SurfaceNormal.Z = -1f; - AddCollisionEvent(0, contact); - - vec.Z *= 0.5f; + m_colliderGroundfilter++; + if (m_colliderGroundfilter > 2) + { + m_iscolliding = true; + m_colliderfilter = 2; + m_colliderGroundfilter = 2; + m_iscollidingGround = true; + + ContactPoint contact = new ContactPoint(); + contact.PenetrationDepth = depth; + contact.Position.X = localpos.X; + contact.Position.Y = localpos.Y; + contact.Position.Z = chrminZ; + contact.SurfaceNormal.X = 0f; + contact.SurfaceNormal.Y = 0f; + contact.SurfaceNormal.Z = -1f; + AddCollisionEvent(0, contact); + + vec.Z *= 0.5f; + } } else + { + m_colliderGroundfilter = 0; m_iscollidingGround = false; + } } else + { + m_colliderGroundfilter = 0; m_iscollidingGround = false; + } //****************************************** bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); - if(!tviszero || m_iscolliding) + // if (!tviszero || m_iscolliding || velLengthSquared <0.01) + if (!tviszero) m_freemove = false; if (!m_freemove) @@ -934,7 +950,6 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - m_freemove = false; m_pidControllerActive = true; _zeroFlag = false; @@ -981,6 +996,24 @@ namespace OpenSim.Region.Physics.OdePlugin } } } + + if (velLengthSquared > 2500.0f) // 50m/s apply breaks + { + breakfactor = 0.16f * m_mass; + vec.X -= breakfactor * vel.X; + vec.Y -= breakfactor * vel.Y; + vec.Z -= breakfactor * vel.Z; + } + } + else + { + breakfactor = 5f * m_mass; + vec.X -= breakfactor * vel.X; + vec.Y -= breakfactor * vel.Y; + if (flying) + vec.Z -= breakfactor * vel.Z; + else + vec.Z -= 2f* m_mass * vel.Z; } if (flying) @@ -997,14 +1030,6 @@ namespace OpenSim.Region.Physics.OdePlugin // end add Kitto Flora } - if (vel.X * vel.X + vel.Y * vel.Y + vel.Z * vel.Z > 2500.0f) // 50m/s apply breaks - { - float breakfactor = 0.16f * m_mass; // will give aprox 60m/s terminal velocity at free fall - vec.X -= breakfactor * vel.X; - vec.Y -= breakfactor * vel.Y; - vec.Z -= breakfactor * vel.Z; - } - if (vec.IsFinite()) { if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) @@ -1222,7 +1247,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); _position = newPos; - m_pidControllerActive = true; + m_pidControllerActive = false; } private void changeOrientation(Quaternion newOri) @@ -1269,11 +1294,30 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeBuilding(bool arg) { - } + } + + private void setFreeMove() + { + m_pidControllerActive = true; + _zeroFlag = false; + _target_velocity = Vector3.Zero; + m_freemove = true; + m_colliderfilter = -2; + m_colliderObjectfilter = -2; + m_colliderGroundfilter = -2; + + m_iscolliding = false; + m_iscollidingGround = false; + m_iscollidingObj = false; + + CollisionEventsThisFrame = new CollisionEventUpdate(); + m_eventsubscription = 0; + } private void changeForce(Vector3 newForce) { - m_pidControllerActive = false; + setFreeMove(); + if (Body != IntPtr.Zero) { if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) @@ -1285,14 +1329,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeMomentum(Vector3 newmomentum) { _velocity = newmomentum; - _target_velocity = Vector3.Zero; - m_freemove = true; - m_pidControllerActive = true; - m_colliderfilter = 0; - m_colliderObjectfilter = 0; - m_iscolliding = false; - m_iscollidingGround = false; - m_iscollidingObj = false; + setFreeMove(); if (Body != IntPtr.Zero) d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); -- cgit v1.1 From 7a7f4b7722f50af874c62894e0fbff413a5a1851 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 09:00:30 +0100 Subject: testing --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index d44c8e6..342b7b3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1247,7 +1247,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); _position = newPos; - m_pidControllerActive = false; + m_pidControllerActive = true; } private void changeOrientation(Quaternion newOri) -- cgit v1.1 From 1c735faceeabc0e2888bdf9b1087a3fa7a9ae094 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 09:23:20 +0100 Subject: test --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 24 ++++++++++------------ 1 file changed, 11 insertions(+), 13 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 342b7b3..36440b1 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -379,24 +379,21 @@ namespace OpenSim.Region.Physics.OdePlugin get { return _position; } set { - if (Body == IntPtr.Zero || Shell == IntPtr.Zero) + if (value.IsFinite()) { - if (value.IsFinite()) + if (value.Z > 9999999f) { - if (value.Z > 9999999f) - { - value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; - } - if (value.Z < -100f) - { - value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; - } - AddChange(changes.Position, value); + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } - else + if (value.Z < -100f) { - m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } + AddChange(changes.Position, value); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); } } } @@ -1248,6 +1245,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); _position = newPos; m_pidControllerActive = true; + m_log.DebugFormat("[ode character new position] {0}", newPos); } private void changeOrientation(Quaternion newOri) -- cgit v1.1 From fedc9eb1056d02c7f99fb4f55a46fdafec02054a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 09:50:53 +0100 Subject: itest --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 36440b1..26f8cf0 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1244,8 +1244,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); _position = newPos; - m_pidControllerActive = true; - m_log.DebugFormat("[ode character new position] {0}", newPos); + m_pidControllerActive = true; } private void changeOrientation(Quaternion newOri) -- cgit v1.1 From 8ef7df5a563a850fea6c1a6f22de55a09cecebe7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 11:01:34 +0100 Subject: test --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 26f8cf0..4f8c4c3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1004,13 +1004,13 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - breakfactor = 5f * m_mass; + breakfactor = m_mass; vec.X -= breakfactor * vel.X; vec.Y -= breakfactor * vel.Y; if (flying) vec.Z -= breakfactor * vel.Z; else - vec.Z -= 2f* m_mass * vel.Z; + vec.Z -= .5f* m_mass * vel.Z; } if (flying) -- cgit v1.1 From ee237fc5dfb42397c0f689b3c101c3be3ed9904c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 12:19:22 +0100 Subject: test --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 4f8c4c3..326fe97 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -880,7 +880,13 @@ namespace OpenSim.Region.Physics.OdePlugin { m_iscolliding = true; m_colliderfilter = 2; - m_colliderGroundfilter = 2; + + if (m_colliderGroundfilter > 10) + { + m_colliderGroundfilter = 10; + m_freemove = false; + } + m_iscollidingGround = true; ContactPoint contact = new ContactPoint(); -- cgit v1.1 From f5cb403e7ec4549c92ba70cb1e2dd1850ce2ae23 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 22:59:57 +0100 Subject: reorder priority of vehicle hover flags --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 223 +++++++++++---------- 1 file changed, 114 insertions(+), 109 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index dcd02e2..53a576e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -121,6 +121,9 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_amEfect = 0; // current angular motor eficiency private float m_ffactor = 1.0f; + private float m_timestep = 0.02f; + private float m_invtimestep = 50; + public float FrictionFactor { get @@ -133,14 +136,12 @@ namespace OpenSim.Region.Physics.OdePlugin { rootPrim = rootp; _pParentScene = rootPrim._parent_scene; + m_timestep = _pParentScene.ODE_STEPSIZE; + m_invtimestep = 1.0f / m_timestep; } public void DoSetVehicle(VehicleData vd) { - - float timestep = _pParentScene.ODE_STEPSIZE; - float invtimestep = 1.0f / timestep; - m_type = vd.m_type; m_flags = vd.m_flags; @@ -148,61 +149,60 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorDirection = vd.m_linearMotorDirection; m_linearFrictionTimescale = vd.m_linearFrictionTimescale; - if (m_linearFrictionTimescale.X < timestep) m_linearFrictionTimescale.X = timestep; - if (m_linearFrictionTimescale.Y < timestep) m_linearFrictionTimescale.Y = timestep; - if (m_linearFrictionTimescale.Z < timestep) m_linearFrictionTimescale.Z = timestep; + if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep; + if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep; + if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep; m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; - if (m_linearMotorDecayTimescale < timestep) m_linearMotorDecayTimescale = timestep; - m_linearMotorDecayTimescale *= invtimestep; + if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep; + m_linearMotorDecayTimescale *= m_invtimestep; m_linearMotorTimescale = vd.m_linearMotorTimescale; - if (m_linearMotorTimescale < timestep) m_linearMotorTimescale = timestep; - + if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep; m_linearMotorOffset = vd.m_linearMotorOffset; //Angular properties m_angularMotorDirection = vd.m_angularMotorDirection; m_angularMotorTimescale = vd.m_angularMotorTimescale; - if (m_angularMotorTimescale < timestep) m_angularMotorTimescale = timestep; + if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep; m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; - if (m_angularMotorDecayTimescale < timestep) m_angularMotorDecayTimescale = timestep; - m_angularMotorDecayTimescale *= invtimestep; + if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep; + m_angularMotorDecayTimescale *= m_invtimestep; m_angularFrictionTimescale = vd.m_angularFrictionTimescale; - if (m_angularFrictionTimescale.X < timestep) m_angularFrictionTimescale.X = timestep; - if (m_angularFrictionTimescale.Y < timestep) m_angularFrictionTimescale.Y = timestep; - if (m_angularFrictionTimescale.Z < timestep) m_angularFrictionTimescale.Z = timestep; + if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep; + if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep; + if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep; //Deflection properties m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; - if (m_angularDeflectionTimescale < timestep) m_angularDeflectionTimescale = timestep; + if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep; m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; - if (m_linearDeflectionTimescale < timestep) m_linearDeflectionTimescale = timestep; + if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep; //Banking properties m_bankingEfficiency = vd.m_bankingEfficiency; m_bankingMix = vd.m_bankingMix; m_bankingTimescale = vd.m_bankingTimescale; - if (m_bankingTimescale < timestep) m_bankingTimescale = timestep; + if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep; //Hover and Buoyancy properties m_VhoverHeight = vd.m_VhoverHeight; m_VhoverEfficiency = vd.m_VhoverEfficiency; m_VhoverTimescale = vd.m_VhoverTimescale; - if (m_VhoverTimescale < timestep) m_VhoverTimescale = timestep; + if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep; m_VehicleBuoyancy = vd.m_VehicleBuoyancy; //Attractor properties m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; - if (m_verticalAttractionTimescale < timestep) m_verticalAttractionTimescale = timestep; + if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep; // Axis m_referenceFrame = vd.m_referenceFrame; @@ -215,8 +215,6 @@ namespace OpenSim.Region.Physics.OdePlugin internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) { float len; - float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE; - float timestep = _pParentScene.ODE_STEPSIZE; switch (pParam) { @@ -226,18 +224,16 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularDeflectionEfficiency = pValue; break; case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_angularDeflectionTimescale = pValue; break; case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < timestep) pValue = timestep; - // try to make impulses to work a bit better -// if (pValue < 0.5f) pValue = 0.5f; + if (pValue < m_timestep) pValue = m_timestep; else if (pValue > 120) pValue = 120; - m_angularMotorDecayTimescale = pValue * invtimestep; + m_angularMotorDecayTimescale = pValue * m_invtimestep; break; case Vehicle.ANGULAR_MOTOR_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_angularMotorTimescale = pValue; break; case Vehicle.BANKING_EFFICIENCY: @@ -251,7 +247,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_bankingMix = pValue; break; case Vehicle.BANKING_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_bankingTimescale = pValue; break; case Vehicle.BUOYANCY: @@ -268,7 +264,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_VhoverHeight = pValue; break; case Vehicle.HOVER_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_VhoverTimescale = pValue; break; case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: @@ -277,18 +273,16 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearDeflectionEfficiency = pValue; break; case Vehicle.LINEAR_DEFLECTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_linearDeflectionTimescale = pValue; break; case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - if (pValue < timestep) pValue = timestep; - // try to make impulses to work a bit better - //if (pValue < 0.5f) pValue = 0.5f; + if (pValue < m_timestep) pValue = m_timestep; else if (pValue > 120) pValue = 120; - m_linearMotorDecayTimescale = pValue * invtimestep; + m_linearMotorDecayTimescale = pValue * m_invtimestep; break; case Vehicle.LINEAR_MOTOR_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_linearMotorTimescale = pValue; break; case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: @@ -297,14 +291,14 @@ namespace OpenSim.Region.Physics.OdePlugin m_verticalAttractionEfficiency = pValue; break; case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_verticalAttractionTimescale = pValue; break; // These are vector properties but the engine lets you use a single float value to // set all of the components to the same value case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: @@ -318,7 +312,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(rootPrim.Body); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; + if (pValue < m_timestep) pValue = m_timestep; m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -344,14 +338,13 @@ namespace OpenSim.Region.Physics.OdePlugin internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) { float len; - float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE; - float timestep = _pParentScene.ODE_STEPSIZE; + switch (pParam) { case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue.X < timestep) pValue.X = timestep; - if (pValue.Y < timestep) pValue.Y = timestep; - if (pValue.Z < timestep) pValue.Z = timestep; + if (pValue.X < m_timestep) pValue.X = m_timestep; + if (pValue.Y < m_timestep) pValue.Y = m_timestep; + if (pValue.Z < m_timestep) pValue.Z = m_timestep; m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; @@ -367,9 +360,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(rootPrim.Body); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue.X < timestep) pValue.X = timestep; - if (pValue.Y < timestep) pValue.Y = timestep; - if (pValue.Z < timestep) pValue.Z = timestep; + if (pValue.X < m_timestep) pValue.X = m_timestep; + if (pValue.Y < m_timestep) pValue.Y = m_timestep; + if (pValue.Z < m_timestep) pValue.Z = m_timestep; m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -422,7 +415,6 @@ namespace OpenSim.Region.Physics.OdePlugin internal void ProcessTypeChange(Vehicle pType) { - float invtimestep = _pParentScene.ODE_STEPSIZE; m_lmEfect = 0; m_amEfect = 0; m_ffactor = 1f; @@ -444,9 +436,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120 * invtimestep; + m_linearMotorDecayTimescale = 120 * m_invtimestep; m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 1000 * invtimestep; + m_angularMotorDecayTimescale = 1000 * m_invtimestep; m_VhoverHeight = 0; m_VhoverEfficiency = 1; m_VhoverTimescale = 1000; @@ -468,9 +460,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(30, 1, 1000); m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120 * invtimestep; + m_linearMotorDecayTimescale = 120 * m_invtimestep; m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 120 * invtimestep; + m_angularMotorDecayTimescale = 120 * m_invtimestep; m_VhoverHeight = 0; m_VhoverEfficiency = 1; m_VhoverTimescale = 10; @@ -491,9 +483,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(100, 2, 1000); m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); m_linearMotorTimescale = 1; - m_linearMotorDecayTimescale = 60 * invtimestep; + m_linearMotorDecayTimescale = 60 * m_invtimestep; m_angularMotorTimescale = 1; - m_angularMotorDecayTimescale = 0.8f * invtimestep; + m_angularMotorDecayTimescale = 0.8f * m_invtimestep; m_VhoverHeight = 0; m_VhoverEfficiency = 0; m_VhoverTimescale = 1000; @@ -515,9 +507,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(10, 3, 2); m_angularFrictionTimescale = new Vector3(10, 10, 10); m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60 * invtimestep; + m_linearMotorDecayTimescale = 60 * m_invtimestep; m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4 * invtimestep; + m_angularMotorDecayTimescale = 4 * m_invtimestep; m_VhoverHeight = 0; m_VhoverEfficiency = 0.5f; m_VhoverTimescale = 2; @@ -543,9 +535,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(200, 10, 5); m_angularFrictionTimescale = new Vector3(20, 20, 20); m_linearMotorTimescale = 2; - m_linearMotorDecayTimescale = 60 * invtimestep; + m_linearMotorDecayTimescale = 60 * m_invtimestep; m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 8 * invtimestep; + m_angularMotorDecayTimescale = 8 * m_invtimestep; m_VhoverHeight = 0; m_VhoverEfficiency = 0.5f; m_VhoverTimescale = 1000; @@ -571,15 +563,15 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearFrictionTimescale = new Vector3(5, 5, 5); m_angularFrictionTimescale = new Vector3(10, 10, 10); m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60 * invtimestep; + m_linearMotorDecayTimescale = 60 * m_invtimestep; m_angularMotorTimescale = 6; - m_angularMotorDecayTimescale = 10 * invtimestep; + m_angularMotorDecayTimescale = 10 * m_invtimestep; m_VhoverHeight = 5; m_VhoverEfficiency = 0.8f; m_VhoverTimescale = 10; m_VehicleBuoyancy = 1; m_linearDeflectionEfficiency = 0; - m_linearDeflectionTimescale = 5 * invtimestep; + m_linearDeflectionTimescale = 5 * m_invtimestep; m_angularDeflectionEfficiency = 0; m_angularDeflectionTimescale = 5; m_verticalAttractionEfficiency = 0f; @@ -701,7 +693,7 @@ namespace OpenSim.Region.Physics.OdePlugin return ; } - internal void Step()//float pTimestep) + internal void Step() { IntPtr Body = rootPrim.Body; @@ -780,38 +772,44 @@ namespace OpenSim.Region.Physics.OdePlugin { d.Vector3 pos = d.BodyGetPosition(Body); - // default to global - float perr = m_VhoverHeight - pos.Z;; - - if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) - { - perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); - } - else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) - { - perr += _pParentScene.GetWaterLevel(); - } - else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) - { - float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); - float w = _pParentScene.GetWaterLevel(); - if (t > w) - perr += t; - else - perr += w; - } + float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); - if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) + if (t < m_VhoverHeight) // don't go underground { - force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE; - force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + // default to global + float perr = m_VhoverHeight - pos.Z; ; + + if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) + { + if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) + { + perr += _pParentScene.GetWaterLevel(); + } + else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) + { + perr += t; + } + else + { + float w = _pParentScene.GetWaterLevel(); + if (t > w) + perr += t; + else + perr += w; + } + } + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) + { + force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep; + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + } + else // no buoyancy + force.Z += _pParentScene.gravityz; } - else // no buoyancy - force.Z += _pParentScene.gravityz; } else { - // default gravity and buoancy + // default gravity and Buoyancy force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); } @@ -819,24 +817,31 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_linearDeflectionEfficiency > 0) { float len = curVel.Length(); - Vector3 atAxis; - atAxis = Xrot(rotq); // where are we pointing to - atAxis *= len; // make it same size as world velocity vector - tmpV = -atAxis; // oposite direction - atAxis -= curVel; // error to one direction - len = atAxis.LengthSquared(); - tmpV -= curVel; // error to oposite - float lens = tmpV.LengthSquared(); - if (len > 0.01 || lens > 0.01) // do nothing if close enougth + if (len > 0.01) // if moving { - if (len < lens) - tmpV = atAxis; - tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep - force.X += tmpV.X; - force.Y += tmpV.Y; - if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) - force.Z += tmpV.Z; + Vector3 atAxis; + atAxis = Xrot(rotq); // where are we pointing to + atAxis *= len; // make it same size as world velocity vector + + tmpV = -atAxis; // oposite direction + atAxis -= curVel; // error to one direction + len = atAxis.LengthSquared(); + + tmpV -= curVel; // error to oposite + float lens = tmpV.LengthSquared(); + + if (len > 0.01 || lens > 0.01) // do nothing if close enougth + { + if (len < lens) + tmpV = atAxis; + + tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep + force.X += tmpV.X; + force.Y += tmpV.Y; + if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) + force.Z += tmpV.Z; + } } } @@ -900,10 +905,10 @@ namespace OpenSim.Region.Physics.OdePlugin GetRollPitch(irotq, out roll, out pitch); - float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; + float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale * m_invtimestep; float ftmp2; if (m_bankingEfficiency == 0) - ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; + ftmp2 = m_verticalAttractionEfficiency * m_invtimestep; else ftmp2 = 0; @@ -985,7 +990,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) { torque *= m_referenceFrame; // to object frame - dtorque.X = torque.X; + dtorque.X = torque.X ; dtorque.Y = torque.Y; dtorque.Z = torque.Z; -- cgit v1.1 From e48fa38ff581bd1125b9582ed3efc875f723ae86 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 27 Apr 2012 23:18:54 +0100 Subject: Oooops don't hover underground but do hover --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 53 +++++++++++----------- 1 file changed, 27 insertions(+), 26 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 53a576e..d106677 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -773,39 +773,40 @@ namespace OpenSim.Region.Physics.OdePlugin d.Vector3 pos = d.BodyGetPosition(Body); float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + float perr; - if (t < m_VhoverHeight) // don't go underground - { - // default to global - float perr = m_VhoverHeight - pos.Z; ; + // default to global but don't go underground + if (t < m_VhoverHeight) + perr = m_VhoverHeight - pos.Z; + else + perr = t - pos.Z; ; - if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) + if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) + { + if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) { - if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) - { - perr += _pParentScene.GetWaterLevel(); - } - else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) - { - perr += t; - } - else - { - float w = _pParentScene.GetWaterLevel(); - if (t > w) - perr += t; - else - perr += w; - } + perr += _pParentScene.GetWaterLevel(); } - if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) + else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) { - force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep; - force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + perr += t; } - else // no buoyancy - force.Z += _pParentScene.gravityz; + else + { + float w = _pParentScene.GetWaterLevel(); + if (t > w) + perr += t; + else + perr += w; + } + } + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) + { + force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep; + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); } + else // no buoyancy + force.Z += _pParentScene.gravityz; } else { -- cgit v1.1 From dd745f60c201aee7ff48ba81d567e7b38a8f186c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 28 Apr 2012 21:36:38 +0100 Subject: fix llGetCenterOfMass ( checked with ubitODE only) --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 35 +++++++++++++++++++++---- 1 file changed, 30 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 2d587ab..c9a453d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -450,7 +450,7 @@ namespace OpenSim.Region.Physics.OdePlugin get { d.Vector3 dtmp; - if (IsPhysical && !childPrim && Body != IntPtr.Zero) + if (!childPrim && Body != IntPtr.Zero) { dtmp = d.BodyGetPosition(Body); return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); @@ -465,12 +465,38 @@ namespace OpenSim.Region.Physics.OdePlugin q.Z = dq.Z; q.W = dq.W; - Vector3 vtmp = primOOBoffset * q; + Vector3 Ptot = primOOBoffset * q; dtmp = d.GeomGetPosition(prim_geom); - return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); + Ptot.X += dtmp.X; + Ptot.Y += dtmp.Y; + Ptot.Z += dtmp.Z; + +// if(childPrim) we only know about physical linksets + return Ptot; +/* + float tmass = _mass; + Ptot *= tmass; + + float m; + + foreach (OdePrim prm in childrenPrim) + { + m = prm._mass; + Ptot += prm.CenterOfMass * m; + tmass += m; + } + + if (tmass == 0) + tmass = 0; + else + tmass = 1.0f / tmass; + + Ptot *= tmass; + return Ptot; + */ } else - return Vector3.Zero; + return _position; } } /* @@ -3490,7 +3516,6 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - return; } else { -- cgit v1.1 From be176b1e4948059e041d06be2caea1eb10b5ee68 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 29 Apr 2012 08:24:41 +0100 Subject: ubitode fix inertia for same cases. Added a nasty lock on llGetCenterOfMass and simulate --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 178 +++++++++++------------ OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 10 +- 2 files changed, 91 insertions(+), 97 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index c9a453d..2bcce31 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -449,54 +449,57 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - d.Vector3 dtmp; - if (!childPrim && Body != IntPtr.Zero) - { - dtmp = d.BodyGetPosition(Body); - return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - } - else if (prim_geom != IntPtr.Zero) + lock (_parent_scene.OdeLock) { - d.Quaternion dq; - d.GeomCopyQuaternion(prim_geom, out dq); - Quaternion q; - q.X = dq.X; - q.Y = dq.Y; - q.Z = dq.Z; - q.W = dq.W; - - Vector3 Ptot = primOOBoffset * q; - dtmp = d.GeomGetPosition(prim_geom); - Ptot.X += dtmp.X; - Ptot.Y += dtmp.Y; - Ptot.Z += dtmp.Z; - -// if(childPrim) we only know about physical linksets - return Ptot; -/* - float tmass = _mass; - Ptot *= tmass; - - float m; - - foreach (OdePrim prm in childrenPrim) + d.Vector3 dtmp; + if (!childPrim && Body != IntPtr.Zero) { - m = prm._mass; - Ptot += prm.CenterOfMass * m; - tmass += m; + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; + + Vector3 Ptot = primOOBoffset * q; + dtmp = d.GeomGetPosition(prim_geom); + Ptot.X += dtmp.X; + Ptot.Y += dtmp.Y; + Ptot.Z += dtmp.Z; + + // if(childPrim) we only know about physical linksets + return Ptot; + /* + float tmass = _mass; + Ptot *= tmass; + + float m; + + foreach (OdePrim prm in childrenPrim) + { + m = prm._mass; + Ptot += prm.CenterOfMass * m; + tmass += m; + } + + if (tmass == 0) + tmass = 0; + else + tmass = 1.0f / tmass; + + Ptot *= tmass; + return Ptot; + */ } - - if (tmass == 0) - tmass = 0; else - tmass = 1.0f / tmass; - - Ptot *= tmass; - return Ptot; - */ + return _position; } - else - return _position; } } /* @@ -1511,7 +1514,6 @@ namespace OpenSim.Region.Physics.OdePlugin } Body = IntPtr.Zero; hasOOBoffsetFromMesh = false; - CalcPrimBodyData(); } /* private void ChildSetGeom(OdePrim odePrim) @@ -1601,7 +1603,7 @@ namespace OpenSim.Region.Physics.OdePlugin Body = d.BodyCreate(_parent_scene.world); - DMassDup(ref primdMass, out objdmass); + objdmass = primdMass; // rotate inertia myrot.X = _orientation.X; @@ -1623,9 +1625,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.Mass tmpdmass = new d.Mass { }; Vector3 rcm; - rcm.X = _position.X + objdmass.c.X; - rcm.Y = _position.Y + objdmass.c.Y; - rcm.Z = _position.Z + objdmass.c.Z; + rcm.X = _position.X; + rcm.Y = _position.Y; + rcm.Z = _position.Z; lock (childrenPrim) { @@ -1637,7 +1639,7 @@ namespace OpenSim.Region.Physics.OdePlugin continue; } - DMassCopy(ref prm.primdMass, ref tmpdmass); + tmpdmass = prm.primdMass; // apply prim current rotation to inertia quat.X = prm._orientation.X; @@ -1648,10 +1650,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.MassRotate(ref tmpdmass, ref mat); Vector3 ppos = prm._position; - ppos.X += tmpdmass.c.X - rcm.X; - ppos.Y += tmpdmass.c.Y - rcm.Y; - ppos.Z += tmpdmass.c.Z - rcm.Z; - + ppos.X -= rcm.X; + ppos.Y -= rcm.Y; + ppos.Z -= rcm.Z; // refer inertia to root prim center of mass position d.MassTranslate(ref tmpdmass, ppos.X, @@ -1683,9 +1684,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - myrot.W = -myrot.W; + myrot.X = -myrot.X; + myrot.Y = -myrot.Y; + myrot.Z = -myrot.Z; + d.RfromQ(out mymat, ref myrot); d.MassRotate(ref objdmass, ref mymat); + d.BodySetMass(Body, ref objdmass); _mass = objdmass.mass; @@ -2237,7 +2242,33 @@ namespace OpenSim.Region.Physics.OdePlugin case ProfileShape.HalfCircle: if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - volume *= 0.52359877559829887307710723054658f; + volume *= 0.5236f; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Circle: + case HollowShape.Triangle: // diference in sl is minor and odd + case HollowShape.Same: + break; + + case HollowShape.Square: + hollowVolume *= 0.909f; + break; + + // case HollowShape.Triangle: + // hollowVolume *= .827f; + // break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } break; @@ -3704,24 +3735,6 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } - internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) - { - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } - internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) { // assumes object center of mass is zero @@ -3745,25 +3758,6 @@ namespace OpenSim.Region.Physics.OdePlugin theobj.I.M22 -= part.I.M22; } - private static void DMassDup(ref d.Mass src, out d.Mass dst) - { - dst = new d.Mass { }; - - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; - } private void donullchange() { } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index fa3d33e..84195d3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -301,7 +301,7 @@ namespace OpenSim.Region.Physics.OdePlugin // split static geometry collision into a grid as before private IntPtr[,] staticPrimspace; - private Object OdeLock; + public Object OdeLock; private static Object SimulationLock; public IMesher mesher; @@ -746,8 +746,7 @@ namespace OpenSim.Region.Physics.OdePlugin ); // do volume detection case if ( - (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) || - (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) + (p1.IsVolumeDtc || p2.IsVolumeDtc)) { collision_accounting_events(p1, p2, maxDepthContact); return; @@ -1024,9 +1023,9 @@ namespace OpenSim.Region.Physics.OdePlugin bool p1events = p1.SubscribedEvents(); bool p2events = p2.SubscribedEvents(); - if (p1 is OdePrim && p1.IsVolumeDtc) + if (p1.IsVolumeDtc) p2events = false; - if (p2 is OdePrim && p2.IsVolumeDtc) + if (p2.IsVolumeDtc) p1events = false; if (!(p2events || p1events)) @@ -1725,6 +1724,7 @@ namespace OpenSim.Region.Physics.OdePlugin // checkThread(); lock (SimulationLock) + lock(OdeLock) { // adjust number of iterations per step try -- cgit v1.1 From 303739622cc9b0f173d9ff88211f28c7295e8c65 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 29 Apr 2012 11:46:16 +0100 Subject: ubitODE fix applyROtationImpulse, let vehicle hover be relative to root prim and not center of mass ( as SL docs said) updated some flags to current ones --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 43 +++++++++++++++------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 4 +- 2 files changed, 32 insertions(+), 15 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index d106677..7b232c1 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -470,15 +470,20 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearDeflectionEfficiency = 1; m_linearDeflectionTimescale = 1; m_angularDeflectionEfficiency = 0; - m_angularDeflectionTimescale = 1000; + m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 1000; m_bankingEfficiency = 0; m_bankingMix = 1; m_bankingTimescale = 10; m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); break; + case Vehicle.TYPE_CAR: m_linearFrictionTimescale = new Vector3(100, 2, 1000); m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); @@ -499,9 +504,13 @@ namespace OpenSim.Region.Physics.OdePlugin m_bankingEfficiency = -0.2f; m_bankingMix = 1; m_bankingTimescale = 1; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_UP_ONLY); break; case Vehicle.TYPE_BOAT: m_linearFrictionTimescale = new Vector3(10, 3, 2); @@ -525,12 +534,14 @@ namespace OpenSim.Region.Physics.OdePlugin m_bankingTimescale = 1; m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT | - VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.LIMIT_ROLL_ONLY); + VehicleFlag.HOVER_UP_ONLY); // | +// VehicleFlag.LIMIT_ROLL_ONLY); m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_UP_ONLY | // new sl VehicleFlag.HOVER_WATER_ONLY); break; + case Vehicle.TYPE_AIRPLANE: m_linearFrictionTimescale = new Vector3(200, 10, 5); m_angularFrictionTimescale = new Vector3(20, 20, 20); @@ -559,6 +570,7 @@ namespace OpenSim.Region.Physics.OdePlugin VehicleFlag.LIMIT_MOTOR_UP); m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); break; + case Vehicle.TYPE_BALLOON: m_linearFrictionTimescale = new Vector3(5, 5, 5); m_angularFrictionTimescale = new Vector3(10, 10, 10); @@ -574,7 +586,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearDeflectionTimescale = 5 * m_invtimestep; m_angularDeflectionEfficiency = 0; m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 0f; + m_verticalAttractionEfficiency = 1f; m_verticalAttractionTimescale = 1000f; m_bankingEfficiency = 0; m_bankingMix = 0.7f; @@ -583,9 +595,12 @@ namespace OpenSim.Region.Physics.OdePlugin VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT); + VehicleFlag.LIMIT_MOTOR_UP | //); + VehicleFlag.LIMIT_ROLL_ONLY | // new sl + VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl + +// m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | +// VehicleFlag.HOVER_GLOBAL_HEIGHT); break; } @@ -768,9 +783,11 @@ namespace OpenSim.Region.Physics.OdePlugin } // hover - if (m_VhoverTimescale < 300) + if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) { - d.Vector3 pos = d.BodyGetPosition(Body); + // d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom); + pos.Z -= 0.21f; // minor offset that seems to be always there in sl float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); float perr; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 2bcce31..e4f2e6b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -843,7 +843,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (force.IsFinite()) { - AddChange(changes.AddAngForce, force * m_invTimeStep); + AddChange(changes.AddAngForce, force); } else { @@ -3217,7 +3217,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeAddImpulse(Vector3 impulse) { - m_forceacc += impulse * m_invTimeStep; + m_forceacc += impulse *m_invTimeStep; if (!m_isSelected) { lock (this) -- cgit v1.1 From a135e51d232143556e8154ccffeaf4860e02e33d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 2 May 2012 22:08:09 +0100 Subject: Improved sitted avatars crossings ( plus tests on vehicles) --- OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 7b232c1..9fefc4e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -793,10 +793,7 @@ namespace OpenSim.Region.Physics.OdePlugin float perr; // default to global but don't go underground - if (t < m_VhoverHeight) - perr = m_VhoverHeight - pos.Z; - else - perr = t - pos.Z; ; + perr = m_VhoverHeight - pos.Z; if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) { @@ -817,9 +814,13 @@ namespace OpenSim.Region.Physics.OdePlugin perr += w; } } + else if (t > m_VhoverHeight) + perr = t - pos.Z; ; + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) { - force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep; + // force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep; + force.Z += (perr / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency);// * m_invtimestep); force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); } else // no buoyancy -- cgit v1.1 From 6b3135aa4dcdcd469054ed0af5eba3b30ae5cd3c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 4 May 2012 22:24:04 +0100 Subject: UbitODE: leave avatar 'freemove' state (entered on setmomentum) on any significant change like new 'velocity' or new position, etc, requests --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 326fe97..672b9e0 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1184,7 +1184,7 @@ namespace OpenSim.Region.Physics.OdePlugin // destroy avatar capsule and related ODE data AvatarGeomAndBodyDestroy(); } - + m_freemove = false; m_isPhysical = NewStatus; } } @@ -1236,7 +1236,7 @@ namespace OpenSim.Region.Physics.OdePlugin + (Amotor == IntPtr.Zero ? "Amotor " : "")); } } - + m_freemove = false; m_pidControllerActive = true; } else @@ -1250,6 +1250,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); _position = newPos; + m_freemove = false; m_pidControllerActive = true; } @@ -1260,6 +1261,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeVelocity(Vector3 newVel) { m_pidControllerActive = true; + m_freemove = false; _target_velocity = newVel; } -- cgit v1.1 From 163a86517ada9ae7f9f1c161192682e02e287e45 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 5 May 2012 03:28:35 +0100 Subject: force lower avatar density for testing --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 672b9e0..b884b62 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -168,6 +168,10 @@ namespace OpenSim.Region.Physics.OdePlugin m_density = density; m_mass = 80f; // sure we have a default + // force lower density for testing + m_density = 3.0f; + + mu = parent_scene.AvatarFriction; walkDivisor = walk_divisor; -- cgit v1.1 From 3b78e33d16e89c4c97f840605f3bbefb5f605835 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 5 May 2012 10:40:03 +0100 Subject: ubitODE prims: - moved outbounds checking back to UpdatePositionAndVelocity() from move() so it's done at end of each ode step and when it reports positions to core. There should be no need to check in both places. - Addforce() and AddAngularForce now apply a force if parameter pushforce is true or apply a impulse if false as it's actually needed (the prims grab case should be a force) --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 95 +++++++++++++++++++++++-- 1 file changed, 91 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index e4f2e6b..87a7e51 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -830,7 +830,10 @@ namespace OpenSim.Region.Physics.OdePlugin { if (force.IsFinite()) { - AddChange(changes.AddForce, force * m_invTimeStep); + if(pushforce) + AddChange(changes.AddForce, force); + else // a impulse + AddChange(changes.AddForce, force * m_invTimeStep); } else { @@ -843,7 +846,10 @@ namespace OpenSim.Region.Physics.OdePlugin { if (force.IsFinite()) { - AddChange(changes.AddAngForce, force); +// if(pushforce) for now applyrotationimpulse seems more happy applied as a force + AddChange(changes.AddAngForce, force); +// else // a impulse +// AddChange(changes.AddAngForce, force * m_invTimeStep); } else { @@ -3375,9 +3381,12 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(Body); } - // check outside region d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator +/* moved down to UpdateMove... where it belongs again + + // check outside region + if (lpos.Z < -100 || lpos.Z > 100000f) { m_outbounds = true; @@ -3453,7 +3462,7 @@ namespace OpenSim.Region.Physics.OdePlugin base.RequestPhysicsterseUpdate(); return; } - +*/ if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) { // 'VEHICLES' are dealt with in ODEDynamics.cs @@ -3507,6 +3516,7 @@ namespace OpenSim.Region.Physics.OdePlugin fx = (_target_velocity.X - vel.X) * m_invTimeStep; fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; +// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); } } // end if (m_usePID) @@ -3622,6 +3632,83 @@ namespace OpenSim.Region.Physics.OdePlugin d.Vector3 lpos = d.GeomGetPosition(prim_geom); + // check outside region + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; + + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if (m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } + d.Quaternion ori; d.GeomCopyQuaternion(prim_geom, out ori); -- cgit v1.1 From 2ab9588c9ad4c3a54d90550f438c9abb0e12a0e2 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 5 May 2012 11:03:38 +0100 Subject: UbitODE: reduced the diference btw dinamic and static friction, making dinamic larger more identical to static. --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 34 ++++++++++++------------ 1 file changed, 17 insertions(+), 17 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 84195d3..ca83dd7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -178,7 +178,7 @@ namespace OpenSim.Region.Physics.OdePlugin const float minERP = 0.1f; const float comumContactCFM = 0.0001f; - float frictionMovementMult = 0.3f; + float frictionMovementMult = 0.8f; float TerrainBounce = 0.1f; float TerrainFriction = 0.3f; @@ -820,18 +820,18 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: -// p1.getContactData(ref contactdata1); -// p2.getContactData(ref contactdata2); + // p1.getContactData(ref contactdata1); + // p2.getContactData(ref contactdata2); bounce = 0; mu = 0; cfm = 0.0001f; -/* - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + /* + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) - mu *= frictionMovementMult; -*/ + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + */ dop2foot = true; if (p1.Velocity.LengthSquared() > 0.0f) p1.CollidingObj = true; @@ -850,14 +850,14 @@ namespace OpenSim.Region.Physics.OdePlugin cfm = p1.Mass; if (cfm > p2.Mass) cfm = p2.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - if (dscale > 1.0f) - dscale = 1.0f; - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + if (dscale > 1.0f) + dscale = 1.0f; + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; @@ -898,7 +898,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } else - ignore=true; + ignore = true; break; } break; -- cgit v1.1 From 75c51f33c4f203fbfee5475872c6d0fdedd83431 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 7 May 2012 21:44:24 +0100 Subject: minor change to linear motor decay --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 28 +++++++++++++--------- 1 file changed, 17 insertions(+), 11 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 9fefc4e..8f2feba 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -118,6 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin // auxiliar private float m_lmEfect = 0; // current linear motor eficiency + private float m_lmDecay = 1.0f; private float m_amEfect = 0; // current angular motor eficiency private float m_ffactor = 1.0f; @@ -155,6 +156,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep; + m_linearMotorDecayTimescale += 0.2f; m_linearMotorDecayTimescale *= m_invtimestep; m_linearMotorTimescale = vd.m_linearMotorTimescale; @@ -208,6 +210,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_referenceFrame = vd.m_referenceFrame; m_lmEfect = 0; + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); m_amEfect = 0; m_ffactor = 1.0f; } @@ -279,7 +282,7 @@ namespace OpenSim.Region.Physics.OdePlugin case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: if (pValue < m_timestep) pValue = m_timestep; else if (pValue > 120) pValue = 120; - m_linearMotorDecayTimescale = pValue * m_invtimestep; + m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep; break; case Vehicle.LINEAR_MOTOR_TIMESCALE: if (pValue < m_timestep) pValue = m_timestep; @@ -318,9 +321,10 @@ namespace OpenSim.Region.Physics.OdePlugin case Vehicle.LINEAR_MOTOR_DIRECTION: m_linearMotorDirection = new Vector3(pValue, pValue, pValue); len = m_linearMotorDirection.Length(); - if (len > 30.0f) - m_linearMotorDirection *= (30.0f / len); - m_lmEfect = 1.0f; // turn it on + if (len > 100.0f) + m_linearMotorDirection *= (100.0f / len); + m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; + m_lmEfect = 1.0f / m_linearMotorTimescale; // turn it on m_ffactor = 0.01f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) @@ -368,9 +372,10 @@ namespace OpenSim.Region.Physics.OdePlugin case Vehicle.LINEAR_MOTOR_DIRECTION: m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); len = m_linearMotorDirection.Length(); - if (len > 30.0f) - m_linearMotorDirection *= (30.0f / len); - m_lmEfect = 1.0f; // turn it on + if (len > 100.0f) + m_linearMotorDirection *= (100.0f / len); + m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; + m_lmEfect = 1.0f / m_linearMotorTimescale; // turn it on m_ffactor = 0.01f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) @@ -603,12 +608,13 @@ namespace OpenSim.Region.Physics.OdePlugin // VehicleFlag.HOVER_GLOBAL_HEIGHT); break; } - + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); }//end SetDefaultsForType internal void Stop() { m_lmEfect = 0; + m_lmDecay = 1.0f; m_amEfect = 0; m_ffactor = 1f; } @@ -739,10 +745,10 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local // linear motor - if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) + if (m_lmEfect > 0.001 && m_linearMotorTimescale < 1000) { tmpV = m_linearMotorDirection - curLocalVel; // velocity error - tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep + tmpV *= m_lmEfect; // error to correct in this timestep tmpV *= rotq; // to world if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) @@ -760,7 +766,7 @@ namespace OpenSim.Region.Physics.OdePlugin force.Y += tmpV.Y; force.Z += tmpV.Z; } - m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale); + m_lmEfect *= m_lmDecay; m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); } -- cgit v1.1 From d0c0d37d241a1586085ea503518163ea345093cd Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 10 May 2012 16:17:02 +0100 Subject: ubitode: changes to vehicles servos --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 254 +++++++++++++-------- 1 file changed, 154 insertions(+), 100 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 8f2feba..e88e559 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -117,14 +117,23 @@ namespace OpenSim.Region.Physics.OdePlugin // auxiliar - private float m_lmEfect = 0; // current linear motor eficiency - private float m_lmDecay = 1.0f; + private float m_lmEfect = 0f; // current linear motor eficiency + private float m_lmDecay = 0f; // current linear decay + private float m_amEfect = 0; // current angular motor eficiency + private float m_amDecay = 0f; // current linear decay + private float m_ffactor = 1.0f; private float m_timestep = 0.02f; private float m_invtimestep = 50; + + float m_ampwr; + float m_amdampX; + float m_amdampY; + float m_amdampZ; + public float FrictionFactor { get @@ -146,6 +155,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_type = vd.m_type; m_flags = vd.m_flags; + // Linear properties m_linearMotorDirection = vd.m_linearMotorDirection; @@ -309,7 +319,10 @@ namespace OpenSim.Region.Physics.OdePlugin len = m_angularMotorDirection.Length(); if (len > 12.566f) m_angularMotorDirection *= (12.566f / len); - m_amEfect = 1.0f; // turn it on + + m_amEfect = 1.0f / m_angularMotorTimescale; // turn it on + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) d.BodyEnable(rootPrim.Body); @@ -323,8 +336,10 @@ namespace OpenSim.Region.Physics.OdePlugin len = m_linearMotorDirection.Length(); if (len > 100.0f) m_linearMotorDirection *= (100.0f / len); + m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; m_lmEfect = 1.0f / m_linearMotorTimescale; // turn it on + m_ffactor = 0.01f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) @@ -358,7 +373,10 @@ namespace OpenSim.Region.Physics.OdePlugin len = m_angularMotorDirection.Length(); if (len > 12.566f) m_angularMotorDirection *= (12.566f / len); - m_amEfect = 1.0f; // turn it on + + m_amEfect = 1.0f / m_angularMotorTimescale; // turn it on + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) d.BodyEnable(rootPrim.Body); @@ -374,8 +392,12 @@ namespace OpenSim.Region.Physics.OdePlugin len = m_linearMotorDirection.Length(); if (len > 100.0f) m_linearMotorDirection *= (100.0f / len); - m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; + m_lmEfect = 1.0f / m_linearMotorTimescale; // turn it on + m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; + + + m_ffactor = 0.01f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) @@ -421,6 +443,7 @@ namespace OpenSim.Region.Physics.OdePlugin internal void ProcessTypeChange(Vehicle pType) { m_lmEfect = 0; + m_amEfect = 0; m_ffactor = 1f; @@ -607,15 +630,20 @@ namespace OpenSim.Region.Physics.OdePlugin // m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | // VehicleFlag.HOVER_GLOBAL_HEIGHT); break; + } + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + }//end SetDefaultsForType internal void Stop() { m_lmEfect = 0; - m_lmDecay = 1.0f; + m_lmDecay = 0f; m_amEfect = 0; + m_amDecay = 0; m_ffactor = 1f; } @@ -642,6 +670,7 @@ namespace OpenSim.Region.Physics.OdePlugin private const float pi = (float)Math.PI; private const float halfpi = 0.5f * (float)Math.PI; + private const float twopi = 2.0f * pi; public static Vector3 ubitRot2Euler(Quaternion rot) { @@ -744,6 +773,8 @@ namespace OpenSim.Region.Physics.OdePlugin curAngVel.Z = dvtmp.Z; Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local + float ldampZ = 0; + // linear motor if (m_lmEfect > 0.001 && m_linearMotorTimescale < 1000) { @@ -766,9 +797,11 @@ namespace OpenSim.Region.Physics.OdePlugin force.Y += tmpV.Y; force.Z += tmpV.Z; } + m_lmEfect *= m_lmDecay; - m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); + // m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); + m_ffactor = 0; } else { @@ -776,18 +809,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_ffactor = 1f; } - // friction - if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) - { - tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; - tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; - tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; - tmpV *= rotq; // to world - force.X += tmpV.X; - force.Y += tmpV.Y; - force.Z += tmpV.Z; - } - // hover if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) { @@ -823,10 +844,16 @@ namespace OpenSim.Region.Physics.OdePlugin else if (t > m_VhoverHeight) perr = t - pos.Z; ; - if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1) { - // force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep; - force.Z += (perr / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency);// * m_invtimestep); + ldampZ = m_VhoverEfficiency * m_invtimestep; + + perr *= (1.0f + ldampZ) / m_VhoverTimescale; + + // force.Z += perr - curVel.Z * tmp; + force.Z += perr; + ldampZ *= -curVel.Z; + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); } else // no buoyancy @@ -844,7 +871,6 @@ namespace OpenSim.Region.Physics.OdePlugin float len = curVel.Length(); if (len > 0.01) // if moving { - Vector3 atAxis; atAxis = Xrot(rotq); // where are we pointing to atAxis *= len; // make it same size as world velocity vector @@ -870,56 +896,19 @@ namespace OpenSim.Region.Physics.OdePlugin } } - // angular motor - if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) - { - tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error - tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep - torque.X += tmpV.X; - torque.Y += tmpV.Y; - torque.Z += tmpV.Z; - m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale); - } - else - m_amEfect = 0; - - // angular friction - if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) - { - torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X; - torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y; - torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z; - } - - // angular deflection - if (m_angularDeflectionEfficiency > 0) + // linear friction/damping + if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) { - Vector3 dirv; - - if (curLocalVel.X > 0.01f) - dirv = curLocalVel; - else if (curLocalVel.X < -0.01f) - // use oposite - dirv = -curLocalVel; - else - { - // make it fall into small positive x case - dirv.X = 0.01f; - dirv.Y = curLocalVel.Y; - dirv.Z = curLocalVel.Z; - } - - float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; - - if (Math.Abs(dirv.Z) > 0.01) - { - torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; - } + tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; + tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; + tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; + tmpV *= rotq; // to world - if (Math.Abs(dirv.Y) > 0.01) - { - torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; - } + if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z)) + tmpV.Z = ldampZ; + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; } // vertical atractor @@ -928,29 +917,30 @@ namespace OpenSim.Region.Physics.OdePlugin float roll; float pitch; - GetRollPitch(irotq, out roll, out pitch); - float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale * m_invtimestep; + + float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale; + float ftmp2; - if (m_bankingEfficiency == 0) - ftmp2 = m_verticalAttractionEfficiency * m_invtimestep; - else - ftmp2 = 0; + ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep; + m_amdampX = ftmp2; + + m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency; + + GetRollPitch(irotq, out roll, out pitch); if (roll > halfpi) roll = pi - roll; else if (roll < -halfpi) - roll = -pi - roll; + roll = -pi - roll; float effroll = pitch / halfpi; effroll *= effroll; effroll = 1 - effroll; effroll *= roll; - if (Math.Abs(effroll) > 0.01) // roll - { - torque.X -= -effroll * ftmp + curLocalAngVel.X * ftmp2; - } + + torque.X += effroll * ftmp; if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0) { @@ -958,24 +948,21 @@ namespace OpenSim.Region.Physics.OdePlugin effpitch *= effpitch; effpitch = 1 - effpitch; effpitch *= pitch; - - if (Math.Abs(effpitch) > 0.01) // pitch - { - torque.Y -= -effpitch * ftmp + curLocalAngVel.Y * ftmp2; - } + + torque.Y += effpitch * ftmp; } if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01) { float broll = effroll; -/* - if (broll > halfpi) - broll = pi - broll; - else if (broll < -halfpi) - broll = -pi - broll; -*/ - broll *= m_bankingEfficiency; + /* + if (broll > halfpi) + broll = pi - broll; + else if (broll < -halfpi) + broll = -pi - broll; + */ + broll *= m_bankingEfficiency; if (m_bankingMix != 0) { float vfact = Math.Abs(curLocalVel.X) / 10.0f; @@ -984,24 +971,91 @@ namespace OpenSim.Region.Physics.OdePlugin if (curLocalVel.X >= 0) broll *= (1 + (vfact - 1) * m_bankingMix); else - broll *= -(1 + (vfact - 1) * m_bankingMix); + broll *= -(1 + (vfact - 1) * m_bankingMix); } // make z rot be in world Z not local as seems to be in sl broll = broll / m_bankingTimescale; - ftmp = -Math.Abs(m_bankingEfficiency) / m_bankingTimescale; - tmpV.X = ftmp * curAngVel.X; - tmpV.Y = ftmp * curAngVel.Y; - tmpV.Z = broll + ftmp * curAngVel.Z; - tmpV *= irotq; + tmpV = Zrot(irotq); + tmpV *= broll; torque.X += tmpV.X; torque.Y += tmpV.Y; torque.Z += tmpV.Z; + + m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale; + m_amdampY = m_amdampZ; + + } + else + { + m_amdampZ = 1 / m_angularFrictionTimescale.Z; + m_amdampY = m_amdampX; + } + } + else + { + m_ampwr = 1.0f; + m_amdampX = 1 / m_angularFrictionTimescale.X; + m_amdampY = 1 / m_angularFrictionTimescale.Y; + m_amdampZ = 1 / m_angularFrictionTimescale.Z; + } + + // angular motor + if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) + { + tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error + tmpV *= m_amEfect; // error to correct in this timestep + torque.X += tmpV.X * m_ampwr; + torque.Y += tmpV.Y * m_ampwr; + torque.Z += tmpV.Z; + + m_amEfect *= m_amDecay; + } + else + m_amEfect = 0; + + // angular deflection + if (m_angularDeflectionEfficiency > 0) + { + Vector3 dirv; + + if (curLocalVel.X > 0.01f) + dirv = curLocalVel; + else if (curLocalVel.X < -0.01f) + // use oposite + dirv = -curLocalVel; + else + { + // make it fall into small positive x case + dirv.X = 0.01f; + dirv.Y = curLocalVel.Y; + dirv.Z = curLocalVel.Z; + } + + float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; + + if (Math.Abs(dirv.Z) > 0.01) + { + torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; + } + + if (Math.Abs(dirv.Y) > 0.01) + { + torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; } } + + // angular friction + if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) + { + torque.X -= curLocalAngVel.X * m_amdampX; + torque.Y -= curLocalAngVel.Y * m_amdampY; + torque.Z -= curLocalAngVel.Z * m_amdampZ; + } + d.Mass dmass; d.BodyGetMass(Body,out dmass); -- cgit v1.1 From 3c37bc2851eb1c8c1ebd164dbf43fbeea427c2b8 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 10 May 2012 22:44:12 +0100 Subject: reduce avatars terminal velocity to less than 30m/s or colisions with basic boxs fail badly. (ode lib problem. chode just may support a bit higher velocity due to the use of tilt). --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 12 +++++++----- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 3 ++- 2 files changed, 9 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b884b62..43b4581 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -734,9 +734,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f); } /// @@ -784,6 +784,8 @@ namespace OpenSim.Region.Physics.OdePlugin // the Amotor still lets avatar rotation to drift during colisions // so force it back to identity + + d.Quaternion qtmp; qtmp.W = 1; qtmp.X = 0; @@ -1004,9 +1006,9 @@ namespace OpenSim.Region.Physics.OdePlugin } } - if (velLengthSquared > 2500.0f) // 50m/s apply breaks + if (velLengthSquared > 625.0f) // 25m/s apply breaks { - breakfactor = 0.16f * m_mass; + breakfactor = 0.31f * m_mass; vec.X -= breakfactor * vel.X; vec.Y -= breakfactor * vel.Y; vec.Z -= breakfactor * vel.Z; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index ca83dd7..e0de6cc 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -536,7 +536,8 @@ namespace OpenSim.Region.Physics.OdePlugin // This is in addition to the step size. // Essentially Steps * m_physicsiterations d.WorldSetQuickStepNumIterations(world, m_physicsiterations); - d.WorldSetContactMaxCorrectingVel(world, 100.0f); + + d.WorldSetContactMaxCorrectingVel(world, 50.0f); spacesPerMeter = 1 / metersInSpace; spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); -- cgit v1.1 From 8dd5f08b6e7d68663307b4346d19ceef711c8425 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 11 May 2012 15:53:31 +0100 Subject: revert terminal vel reduction. It helped but not efective --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 43b4581..b0711d7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1006,9 +1006,9 @@ namespace OpenSim.Region.Physics.OdePlugin } } - if (velLengthSquared > 625.0f) // 25m/s apply breaks + if (velLengthSquared > 2500.0f) // 50m/s apply breaks { - breakfactor = 0.31f * m_mass; + breakfactor = 0.16f * m_mass; vec.X -= breakfactor * vel.X; vec.Y -= breakfactor * vel.Y; vec.Z -= breakfactor * vel.Z; -- cgit v1.1 From 46095c963c633ef7e690fc033d5e7213fa4ed46e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 12 May 2012 12:17:28 +0100 Subject: ubitODE: trial workaround for avatar colisions --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 55 +++++++++++++++++++++--- 1 file changed, 49 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index e0de6cc..63462b1 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -537,7 +537,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Essentially Steps * m_physicsiterations d.WorldSetQuickStepNumIterations(world, m_physicsiterations); - d.WorldSetContactMaxCorrectingVel(world, 50.0f); + d.WorldSetContactMaxCorrectingVel(world, 100.0f); spacesPerMeter = 1 / metersInSpace; spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); @@ -754,14 +754,15 @@ namespace OpenSim.Region.Physics.OdePlugin } // big messy collision analises + + Vector3 normoverride = Vector3.Zero; //damm c# + float mu = 0; float bounce = 0; float cfm = 0.0001f; - float erp = 0.1f; float erpscale = 1.0f; float dscale = 1.0f; bool IgnoreNegSides = false; - ContactData contactdata1 = new ContactData(0, 0, false); ContactData contactdata2 = new ContactData(0, 0, false); @@ -770,14 +771,30 @@ namespace OpenSim.Region.Physics.OdePlugin bool dop1foot = false; bool dop2foot = false; bool ignore = false; + bool AvanormOverride = false; switch (p1.PhysicsActorType) { case (int)ActorTypes.Agent: { - bounce = 0; - mu = 0; - cfm = 0.0001f; + AvanormOverride = true; + Vector3 tmp = p2.Position - p1.Position; + normoverride = p2.Velocity - p1.Velocity; + mu = normoverride.LengthSquared(); + + if (mu > 1e-6) + { + mu = 1.0f / (float)Math.Sqrt(mu); + normoverride *= mu; + mu = Vector3.Dot(tmp, normoverride); + if (mu > 0) + normoverride *= -1; + } + else + { + tmp.Normalize(); + normoverride = -tmp; + } switch (p2.PhysicsActorType) { @@ -824,6 +841,25 @@ namespace OpenSim.Region.Physics.OdePlugin // p1.getContactData(ref contactdata1); // p2.getContactData(ref contactdata2); + AvanormOverride = true; + + Vector3 tmp = p2.Position - p1.Position; + normoverride = p2.Velocity - p1.Velocity; + mu = normoverride.LengthSquared(); + if (mu > 1e-6) + { + mu = 1.0f / (float)Math.Sqrt(mu); + normoverride *= mu; + mu = Vector3.Dot(tmp, normoverride); + if (mu > 0) + normoverride *= -1; + } + else + { + tmp.Normalize(); + normoverride = -tmp; + } + bounce = 0; mu = 0; cfm = 0.0001f; @@ -974,6 +1010,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) p2.IsColliding = true; + if (AvanormOverride && curContact.depth > 0.3f) + { + curContact.normal.X = normoverride.X; + curContact.normal.Y = normoverride.Y; + curContact.normal.Z = normoverride.Z; + } + Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); d.JointAttach(Joint, b1, b2); -- cgit v1.1 From 792e8db45695a6151c4e7d039b792bdfeb5c0f87 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 12 May 2012 13:44:47 +0100 Subject: ubitODE reduced again a bit the max allowed correction velocity on colisions, to reduce a bit bouncing inerent to colisions. --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 63462b1..7367719 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -537,7 +537,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Essentially Steps * m_physicsiterations d.WorldSetQuickStepNumIterations(world, m_physicsiterations); - d.WorldSetContactMaxCorrectingVel(world, 100.0f); + d.WorldSetContactMaxCorrectingVel(world, 60.0f); spacesPerMeter = 1 / metersInSpace; spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); -- cgit v1.1 From 4d98a291a2d1440afd8f7375d14842fd91d8083f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 12 May 2012 14:00:08 +0100 Subject: ubitODE let vehicles responde faster to changes of some parameters like motors decay times --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 25 +++++++++++----------- 1 file changed, 13 insertions(+), 12 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index e88e559..56d0f1a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -63,6 +63,9 @@ namespace OpenSim.Region.Physics.OdePlugin private OdeScene _pParentScene; // Vehicle properties + // WARNING this are working copies for internel use + // their values may not be the corresponding parameter + private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? @@ -244,6 +247,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (pValue < m_timestep) pValue = m_timestep; else if (pValue > 120) pValue = 120; m_angularMotorDecayTimescale = pValue * m_invtimestep; + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; break; case Vehicle.ANGULAR_MOTOR_TIMESCALE: if (pValue < m_timestep) pValue = m_timestep; @@ -293,6 +297,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (pValue < m_timestep) pValue = m_timestep; else if (pValue > 120) pValue = 120; m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep; + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); break; case Vehicle.LINEAR_MOTOR_TIMESCALE: if (pValue < m_timestep) pValue = m_timestep; @@ -320,7 +325,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 12.566f) m_angularMotorDirection *= (12.566f / len); - m_amEfect = 1.0f / m_angularMotorTimescale; // turn it on + m_amEfect = 1.0f ; // turn it on m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) @@ -338,7 +343,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorDirection *= (100.0f / len); m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; - m_lmEfect = 1.0f / m_linearMotorTimescale; // turn it on + m_lmEfect = 1.0f; // turn it on m_ffactor = 0.01f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) @@ -374,7 +379,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 12.566f) m_angularMotorDirection *= (12.566f / len); - m_amEfect = 1.0f / m_angularMotorTimescale; // turn it on + m_amEfect = 1.0f; // turn it on m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) @@ -393,11 +398,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (len > 100.0f) m_linearMotorDirection *= (100.0f / len); - m_lmEfect = 1.0f / m_linearMotorTimescale; // turn it on + m_lmEfect = 1.0f; // turn it on m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; - - m_ffactor = 0.01f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) @@ -776,10 +779,10 @@ namespace OpenSim.Region.Physics.OdePlugin float ldampZ = 0; // linear motor - if (m_lmEfect > 0.001 && m_linearMotorTimescale < 1000) + if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) { tmpV = m_linearMotorDirection - curLocalVel; // velocity error - tmpV *= m_lmEfect; // error to correct in this timestep + tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep tmpV *= rotq; // to world if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) @@ -799,9 +802,7 @@ namespace OpenSim.Region.Physics.OdePlugin } m_lmEfect *= m_lmDecay; - - // m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); - m_ffactor = 0; + m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); } else { @@ -1007,7 +1008,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) { tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error - tmpV *= m_amEfect; // error to correct in this timestep + tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep torque.X += tmpV.X * m_ampwr; torque.Y += tmpV.Y * m_ampwr; torque.Z += tmpV.Z; -- cgit v1.1 From 9870d7e4e787ca64011ef817ea2ab40310f4cf26 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 13 May 2012 01:28:20 +0100 Subject: ubitODE fix force in case of mlinear motor offset present --- OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 56d0f1a..e27be1e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -750,6 +750,9 @@ namespace OpenSim.Region.Physics.OdePlugin { IntPtr Body = rootPrim.Body; + d.Mass dmass; + d.BodyGetMass(Body, out dmass); + d.Quaternion rot = d.BodyGetQuaternion(Body); Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object Quaternion rotq = objrotq; // rotq = rotation of object @@ -791,7 +794,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0) { // have offset, do it now - tmpV *= rootPrim.Mass; + tmpV *= dmass.mass; d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); } else @@ -1058,13 +1061,11 @@ namespace OpenSim.Region.Physics.OdePlugin } - d.Mass dmass; - d.BodyGetMass(Body,out dmass); if (force.X != 0 || force.Y != 0 || force.Z != 0) { force *= dmass.mass; - d.BodySetForce(Body, force.X, force.Y, force.Z); + d.BodyAddForce(Body, force.X, force.Y, force.Z); } if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) -- cgit v1.1 From 338be76e0a5974b95db2618b8318b76f6fe3e8a4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 15 May 2012 13:41:13 +0100 Subject: ubitODE: fix not reporting land collisions on same cases. --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 27 ++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 7367719..dd912da 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -620,8 +620,6 @@ namespace OpenSim.Region.Physics.OdePlugin return d.JointCreateContactPtr(world, contactgroup, contact); } - - private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) { if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) @@ -1153,6 +1151,31 @@ namespace OpenSim.Region.Physics.OdePlugin } } break; + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + if (p2 is OdeCharacter) + { + cc2 = (OdeCharacter)p2; + obj2LocalID = cc2.m_localID; + if (p2events) + cc2.AddCollisionEvent(0, contact); + } + break; + case ActorTypes.Prim: + if (p2 is OdePrim) + { + cp2 = (OdePrim)p2; + obj2LocalID = cp2.m_localID; + if (p2events) + cp2.AddCollisionEvent(0, contact); + } + break; + } + break; } } -- cgit v1.1 From 9d675232359a30c56d2b68d50fe15958374ec773 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 15 May 2012 15:45:01 +0100 Subject: ubitODE: if stopped having collisions do report zero colisions once, so collision_end event can be triggered. Changed reports rate to the requested rate and not full ode rate. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 46 +++++++++++++++++++----- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 6 +++- 2 files changed, 43 insertions(+), 9 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 87a7e51..5db4f17 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -194,8 +194,10 @@ namespace OpenSim.Region.Physics.OdePlugin public int givefakeori = 0; private Quaternion fakeori; - public int m_eventsubscription; - private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + private int m_eventsubscription; + private int m_cureventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = null; + private bool SentEmptyCollisionsEvent; public volatile bool childPrim; @@ -931,12 +933,21 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SubscribeEvents(int ms) { m_eventsubscription = ms; + m_cureventsubscription = 0; + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + SentEmptyCollisionsEvent = false; _parent_scene.AddCollisionEventReporting(this); } public override void UnSubscribeEvents() { _parent_scene.RemoveCollisionEventReporting(this); + if (CollisionEventsThisFrame != null) + { + CollisionEventsThisFrame.Clear(); + CollisionEventsThisFrame = null; + } m_eventsubscription = 0; } @@ -944,7 +955,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (CollisionEventsThisFrame == null) CollisionEventsThisFrame = new CollisionEventUpdate(); - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); } @@ -953,14 +963,34 @@ namespace OpenSim.Region.Physics.OdePlugin if (CollisionEventsThisFrame == null) return; - base.SendCollisionUpdate(CollisionEventsThisFrame); + if (m_cureventsubscription < m_eventsubscription) + return; - if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) - CollisionEventsThisFrame = null; - else - CollisionEventsThisFrame = new CollisionEventUpdate(); + m_cureventsubscription = 0; + + int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + + if (!SentEmptyCollisionsEvent || ncolisions > 0) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (ncolisions == 0) + SentEmptyCollisionsEvent = true; + else + { + SentEmptyCollisionsEvent = false; + CollisionEventsThisFrame.Clear(); + } + } } + internal void AddCollisionFrameTime(int t) + { + // protect it from overflow crashing + if (m_cureventsubscription + t >= int.MaxValue) + m_cureventsubscription = 0; + m_cureventsubscription += t; + } public override bool SubscribedEvents() { if (m_eventsubscription > 0) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index dd912da..ddfdea4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1898,7 +1898,11 @@ namespace OpenSim.Region.Physics.OdePlugin case ActorTypes.Prim: OdePrim pobj = (OdePrim)obj; - pobj.SendCollisions(); + if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) + { + pobj.AddCollisionFrameTime((int)(ODE_STEPSIZE * 1000.0f)); + pobj.SendCollisions(); + } break; } } -- cgit v1.1 From a7ece8c688a44c0d0b05162dbb1e98a1ea4e95ff Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 19 May 2012 00:17:37 +0100 Subject: add colliders relative velocity projected in collision direction to collisions report information. --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 2 ++ OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 11 ++++++++++- 3 files changed, 13 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index b66d7f1..fb90887 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -59,12 +59,14 @@ namespace OpenSim.Region.Physics.Manager public Vector3 Position; public Vector3 SurfaceNormal; public float PenetrationDepth; + public float RelativeSpeed; public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth) { Position = position; SurfaceNormal = surfaceNormal; PenetrationDepth = penetrationDepth; + RelativeSpeed = 0f; // for now let this one be set explicity } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b0711d7..bfff3d4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -903,6 +903,7 @@ namespace OpenSim.Region.Physics.OdePlugin contact.SurfaceNormal.X = 0f; contact.SurfaceNormal.Y = 0f; contact.SurfaceNormal.Z = -1f; + contact.RelativeSpeed = -vel.Z; AddCollisionEvent(0, contact); vec.Z *= 0.5f; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index ddfdea4..11638d7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1064,7 +1064,7 @@ namespace OpenSim.Region.Physics.OdePlugin obj2LocalID = 0; bool p1events = p1.SubscribedEvents(); bool p2events = p2.SubscribedEvents(); - + if (p1.IsVolumeDtc) p2events = false; if (p2.IsVolumeDtc) @@ -1073,6 +1073,15 @@ namespace OpenSim.Region.Physics.OdePlugin if (!(p2events || p1events)) return; + Vector3 vel = Vector3.Zero; + if (p2 != null && p2.IsPhysical) + vel = p2.Velocity; + + if (p1 != null && p1.IsPhysical) + vel -= p1.Velocity; + + contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); + switch ((ActorTypes)p1.PhysicsActorType) { case ActorTypes.Agent: -- cgit v1.1 From deb87e78907fae00301c9c3428da2f41e1f588b3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 19 May 2012 01:01:46 +0100 Subject: fix character IsPhysical --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index bfff3d4..b36b933 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -239,7 +239,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool IsPhysical { - get { return false; } + get { return m_isPhysical; } set { return; } } -- cgit v1.1 From 10889c86d9d67ca994a090166ad53840ed1dedd0 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 19 May 2012 16:35:48 +0100 Subject: reduce useless waste of cpu. Make character collision events be done similiar to parts. Let same thread do it all ( like in parts ) ( to change this some structs copies must be added) --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 79 ++++++++------ OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 121 +++++++++++---------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 70 ++++++------ 3 files changed, 145 insertions(+), 125 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b36b933..ca294b8 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -128,9 +128,10 @@ namespace OpenSim.Region.Physics.OdePlugin public d.Mass ShellMass; // public bool collidelock = false; - private bool m_haseventsubscription = false; public int m_eventsubscription = 0; - private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + private int m_cureventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = null; + private bool SentEmptyCollisionsEvent; // unique UUID of this character object public UUID m_uuid; @@ -1120,47 +1121,72 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SubscribeEvents(int ms) { - m_requestedUpdateFrequency = ms; m_eventsubscription = ms; - _parent_scene.AddCollisionEventReporting(this); - m_haseventsubscription = true; + m_cureventsubscription = 0; + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + SentEmptyCollisionsEvent = false; } public override void UnSubscribeEvents() { - m_haseventsubscription = false; - _parent_scene.RemoveCollisionEventReporting(this); - m_requestedUpdateFrequency = 0; + if (CollisionEventsThisFrame != null) + { + CollisionEventsThisFrame.Clear(); + CollisionEventsThisFrame = null; + } m_eventsubscription = 0; } public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { - if (m_haseventsubscription) - { - // m_log.DebugFormat( - // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact); - - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - } + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); } public void SendCollisions() { - if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency) + if (CollisionEventsThisFrame == null) + return; + + if (m_cureventsubscription < m_eventsubscription) + return; + + m_cureventsubscription = 0; + + int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + + if (!SentEmptyCollisionsEvent || ncolisions > 0) { - if (CollisionEventsThisFrame != null) + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (ncolisions == 0) { - base.SendCollisionUpdate(CollisionEventsThisFrame); + SentEmptyCollisionsEvent = true; + _parent_scene.RemoveCollisionEventReporting(this); } - CollisionEventsThisFrame = new CollisionEventUpdate(); - m_eventsubscription = 0; - } + else + { + SentEmptyCollisionsEvent = false; + CollisionEventsThisFrame.Clear(); + } + } + } + + internal void AddCollisionFrameTime(int t) + { + // protect it from overflow crashing + if (m_cureventsubscription + t >= int.MaxValue) + m_cureventsubscription = 0; + m_cureventsubscription += t; } public override bool SubscribedEvents() { - return m_haseventsubscription; + if (m_eventsubscription > 0) + return true; + return false; } private void changePhysicsStatus(bool NewStatus) @@ -1466,14 +1492,5 @@ namespace OpenSim.Region.Physics.OdePlugin { _parent_scene.AddChange((PhysicsActor)this, what, arg); } - - - internal void AddCollisionFrameTime(int p) - { - // protect it from overflow crashing - if (m_eventsubscription + p >= int.MaxValue) - m_eventsubscription = 0; - m_eventsubscription += p; - } } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 5db4f17..b2af180 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -477,58 +477,58 @@ namespace OpenSim.Region.Physics.OdePlugin // if(childPrim) we only know about physical linksets return Ptot; - /* - float tmass = _mass; - Ptot *= tmass; +/* + float tmass = _mass; + Ptot *= tmass; - float m; + float m; - foreach (OdePrim prm in childrenPrim) - { - m = prm._mass; - Ptot += prm.CenterOfMass * m; - tmass += m; - } + foreach (OdePrim prm in childrenPrim) + { + m = prm._mass; + Ptot += prm.CenterOfMass * m; + tmass += m; + } - if (tmass == 0) - tmass = 0; - else - tmass = 1.0f / tmass; + if (tmass == 0) + tmass = 0; + else + tmass = 1.0f / tmass; - Ptot *= tmass; - return Ptot; - */ + Ptot *= tmass; + return Ptot; +*/ } else return _position; } } } - /* - public override Vector3 PrimOOBsize - { - get - { - return primOOBsize; - } - } +/* + public override Vector3 PrimOOBsize + { + get + { + return primOOBsize; + } + } - public override Vector3 PrimOOBoffset - { - get - { - return primOOBoffset; - } - } + public override Vector3 PrimOOBoffset + { + get + { + return primOOBoffset; + } + } - public override float PrimOOBRadiusSQ - { - get - { - return primOOBradiusSQ; - } - } - */ + public override float PrimOOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } + } +*/ public override PrimitiveBaseShape Shape { set @@ -582,7 +582,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { AddChange(changes.Velocity, value); - // _velocity = value; +// _velocity = value; } else @@ -937,12 +937,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (CollisionEventsThisFrame == null) CollisionEventsThisFrame = new CollisionEventUpdate(); SentEmptyCollisionsEvent = false; - _parent_scene.AddCollisionEventReporting(this); } public override void UnSubscribeEvents() { - _parent_scene.RemoveCollisionEventReporting(this); if (CollisionEventsThisFrame != null) { CollisionEventsThisFrame.Clear(); @@ -975,7 +973,10 @@ namespace OpenSim.Region.Physics.OdePlugin base.SendCollisionUpdate(CollisionEventsThisFrame); if (ncolisions == 0) + { SentEmptyCollisionsEvent = true; + _parent_scene.RemoveCollisionEventReporting(this); + } else { SentEmptyCollisionsEvent = false; @@ -1735,8 +1736,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - // d.BodySetLinearDampingThreshold(Body, 0.01f); - // d.BodySetAngularDampingThreshold(Body, 0.001f); +// d.BodySetLinearDampingThreshold(Body, 0.01f); +// d.BodySetAngularDampingThreshold(Body, 0.001f); d.BodySetDamping(Body, .002f, .002f); if (m_targetSpace != IntPtr.Zero) @@ -2966,7 +2967,7 @@ namespace OpenSim.Region.Physics.OdePlugin givefakepos--; if (givefakepos < 0) givefakepos = 0; - // changeSelectedStatus(); +// changeSelectedStatus(); resetCollisionAccounting(); } @@ -2981,14 +2982,14 @@ namespace OpenSim.Region.Physics.OdePlugin { _orientation = newOri; } - /* - else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) - { - FixInertia(_position, newOri); - if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - */ +/* + else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) + { + FixInertia(_position, newOri); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } +*/ } else { @@ -3939,12 +3940,12 @@ namespace OpenSim.Region.Physics.OdePlugin changevelocity((Vector3)arg); break; - // case changes.Acceleration: - // changeacceleration((Vector3)arg); - // break; - // case changes.AngVelocity: - // changeangvelocity((Vector3)arg); - // break; +// case changes.Acceleration: +// changeacceleration((Vector3)arg); +// break; +// case changes.AngVelocity: +// changeangvelocity((Vector3)arg); +// break; case changes.Force: changeForce((Vector3)arg); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 11638d7..0f341b9 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -247,6 +247,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// A list of actors that should receive collision events. /// private readonly List _collisionEventPrim = new List(); + private readonly List _collisionEventPrimRemove = new List(); private readonly HashSet _badCharacter = new HashSet(); public Dictionary geom_name_map = new Dictionary(); @@ -1073,6 +1074,12 @@ namespace OpenSim.Region.Physics.OdePlugin if (!(p2events || p1events)) return; + if (p1events) + AddCollisionEventReporting(p1); + + if (p2events) + AddCollisionEventReporting(p2); + Vector3 vel = Vector3.Zero; if (p2 != null && p2.IsPhysical) vel = p2.Velocity; @@ -1255,20 +1262,14 @@ namespace OpenSim.Region.Physics.OdePlugin } #endregion - - - /// /// Add actor to the list that should receive collision events in the simulate loop. /// /// public void AddCollisionEventReporting(PhysicsActor obj) { - lock (_collisionEventPrim) - { - if (!_collisionEventPrim.Contains(obj)) - _collisionEventPrim.Add(obj); - } + if (!_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Add(obj); } /// @@ -1277,13 +1278,11 @@ namespace OpenSim.Region.Physics.OdePlugin /// public void RemoveCollisionEventReporting(PhysicsActor obj) { - lock (_collisionEventPrim) - { - if (_collisionEventPrim.Contains(obj)) - _collisionEventPrim.Remove(obj); - } + if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) + _collisionEventPrimRemove.Add(obj); } + #region Add/Remove Entities public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) @@ -1472,6 +1471,7 @@ namespace OpenSim.Region.Physics.OdePlugin { // lock (OdeLock) { + OdePrim p = (OdePrim)prim; p.setPrimForRemoval(); } @@ -1890,33 +1890,35 @@ namespace OpenSim.Region.Physics.OdePlugin collision_optimized(); - lock (_collisionEventPrim) + foreach (PhysicsActor obj in _collisionEventPrim) { - foreach (PhysicsActor obj in _collisionEventPrim) + if (obj == null) + continue; + + switch ((ActorTypes)obj.PhysicsActorType) { - if (obj == null) - continue; + case ActorTypes.Agent: + OdeCharacter cobj = (OdeCharacter)obj; + cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE * 1000.0f)); + cobj.SendCollisions(); + break; - switch ((ActorTypes)obj.PhysicsActorType) - { - case ActorTypes.Agent: - OdeCharacter cobj = (OdeCharacter)obj; - cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f)); - cobj.SendCollisions(); - break; - - case ActorTypes.Prim: - OdePrim pobj = (OdePrim)obj; - if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) - { - pobj.AddCollisionFrameTime((int)(ODE_STEPSIZE * 1000.0f)); - pobj.SendCollisions(); - } - break; - } + case ActorTypes.Prim: + OdePrim pobj = (OdePrim)obj; + if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) + { + pobj.AddCollisionFrameTime((int)(ODE_STEPSIZE * 1000.0f)); + pobj.SendCollisions(); + } + break; } } + foreach (PhysicsActor obj in _collisionEventPrimRemove) + _collisionEventPrim.Remove(obj); + + _collisionEventPrimRemove.Clear(); + // do a ode simulation step d.WorldQuickStep(world, ODE_STEPSIZE); d.JointGroupEmpty(contactgroup); -- cgit v1.1 From 11f582b26da987afe4c9ad76c6ce35b58a8bc6fd Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 20 May 2012 13:18:15 +0100 Subject: minor changes --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 7 ++-- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 7 ++-- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 42 ++++++++++------------ 3 files changed, 24 insertions(+), 32 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index ca294b8..1084b0e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -784,8 +784,6 @@ namespace OpenSim.Region.Physics.OdePlugin // the Amotor still lets avatar rotation to drift during colisions // so force it back to identity - - d.Quaternion qtmp; qtmp.W = 1; @@ -1177,9 +1175,8 @@ namespace OpenSim.Region.Physics.OdePlugin internal void AddCollisionFrameTime(int t) { // protect it from overflow crashing - if (m_cureventsubscription + t >= int.MaxValue) - m_cureventsubscription = 0; - m_cureventsubscription += t; + if (m_cureventsubscription < 50000) + m_cureventsubscription += t; } public override bool SubscribedEvents() diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index b2af180..62fd279 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -987,11 +987,10 @@ namespace OpenSim.Region.Physics.OdePlugin internal void AddCollisionFrameTime(int t) { - // protect it from overflow crashing - if (m_cureventsubscription + t >= int.MaxValue) - m_cureventsubscription = 0; - m_cureventsubscription += t; + if (m_cureventsubscription < 50000) + m_cureventsubscription += t; } + public override bool SubscribedEvents() { if (m_eventsubscription > 0) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 0f341b9..bfcfd21 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -190,6 +190,7 @@ namespace OpenSim.Region.Physics.OdePlugin public float ODE_STEPSIZE = 0.020f; public float HalfOdeStep = 0.01f; + public int odetimestepMS = 20; // rounded private float metersInSpace = 25.6f; private float m_timeDilation = 1.0f; @@ -490,6 +491,7 @@ namespace OpenSim.Region.Physics.OdePlugin } HalfOdeStep = ODE_STEPSIZE * 0.5f; + odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); @@ -1827,7 +1829,6 @@ namespace OpenSim.Region.Physics.OdePlugin { int ttmpstart = Util.EnvironmentTickCount(); int ttmp; - int ttmp2; while(ChangesQueue.Dequeue(out item)) { @@ -1849,11 +1850,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (ttmp > 20) break; } - - ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart); - if (ttmp2 > 50) - ttmp2 = 0; - } // Move characters @@ -1899,7 +1895,7 @@ namespace OpenSim.Region.Physics.OdePlugin { case ActorTypes.Agent: OdeCharacter cobj = (OdeCharacter)obj; - cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE * 1000.0f)); + cobj.AddCollisionFrameTime((int)(odetimestepMS)); cobj.SendCollisions(); break; @@ -1907,7 +1903,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim pobj = (OdePrim)obj; if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) { - pobj.AddCollisionFrameTime((int)(ODE_STEPSIZE * 1000.0f)); + pobj.AddCollisionFrameTime((int)(odetimestepMS)); pobj.SendCollisions(); } break; @@ -1924,21 +1920,21 @@ namespace OpenSim.Region.Physics.OdePlugin d.JointGroupEmpty(contactgroup); // update managed ideia of physical data and do updates to core - /* - lock (_characters) - { - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - { - if (actor.bad) - m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); - - actor.UpdatePositionAndVelocity(); - } - } - } - */ + /* + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); + + actor.UpdatePositionAndVelocity(); + } + } + } + */ lock (_activegroups) { -- cgit v1.1 From 85f578999320ab4b3b87d99c6901e7dfc86f8cc3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 21 May 2012 12:35:17 +0100 Subject: try to let avas climb higher steps. Will only work in some cases, may have bad effects, so needs some more testing --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 82 +++++++++++++++++++++--- 2 files changed, 75 insertions(+), 9 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 62fd279..459cd27 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2562,10 +2562,10 @@ namespace OpenSim.Region.Physics.OdePlugin { d.Quaternion qtmp; d.GeomCopyQuaternion(prim_geom, out qtmp); - _orientation.W = qtmp.W; _orientation.X = qtmp.X; _orientation.Y = qtmp.Y; _orientation.Z = qtmp.Z; + _orientation.W = qtmp.W; d.Vector3 lpos = d.GeomGetPosition(prim_geom); _position.X = lpos.X; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index bfcfd21..d5968fc 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1006,16 +1006,82 @@ namespace OpenSim.Region.Physics.OdePlugin else { - if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) - p1.IsColliding = true; - if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) - p2.IsColliding = true; - if (AvanormOverride && curContact.depth > 0.3f) + if (AvanormOverride) { - curContact.normal.X = normoverride.X; - curContact.normal.Y = normoverride.Y; - curContact.normal.Z = normoverride.Z; + if (curContact.depth > 0.3f) + { + if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) + p1.IsColliding = true; + if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) + p2.IsColliding = true; + curContact.normal.X = normoverride.X; + curContact.normal.Y = normoverride.Y; + curContact.normal.Z = normoverride.Z; + } + + else + { + if (dop1foot) + { + float sz = p1.Size.Z; + Vector3 vtmp = p1.Position; + float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; + if (ppos > 0f) + { + if (!p1.Flying) + { + d.AABB aabb; + d.GeomGetAABB(g2, out aabb); + float tmp = vtmp.Z - sz * .25f; + + if (aabb.MaxZ < tmp) + { + vtmp.X = curContact.pos.X - vtmp.X; + vtmp.Y = curContact.pos.Y - vtmp.Y; + vtmp.Z = -0.2f; + vtmp.Normalize(); + curContact.normal.X = vtmp.X; + curContact.normal.Y = vtmp.Y; + curContact.normal.Z = vtmp.Z; + } + } + } + else + p1.IsColliding = true; + + } + + if (dop2foot) + { + float sz = p2.Size.Z; + Vector3 vtmp = p2.Position; + float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; + if (ppos > 0f) + { + if (!p2.Flying) + { + d.AABB aabb; + d.GeomGetAABB(g1, out aabb); + float tmp = vtmp.Z - sz * .25f; + + if (aabb.MaxZ < tmp) + { + vtmp.X = curContact.pos.X - vtmp.X; + vtmp.Y = curContact.pos.Y - vtmp.Y; + vtmp.Z = -0.2f; + vtmp.Normalize(); + curContact.normal.X = vtmp.X; + curContact.normal.Y = vtmp.Y; + curContact.normal.Z = vtmp.Z; + } + } + } + else + p2.IsColliding = true; + + } + } } Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); -- cgit v1.1 From d2260423e5e955b4e0f33f279e64cf74d580dc37 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 21 May 2012 17:11:19 +0100 Subject: also don't collide sculps or meshs if meshing is OFF --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 ++ 1 file changed, 2 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 459cd27..496e7ac 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1482,6 +1482,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (!haveMesh) m_NoColide = true; } + else if(_pbs.SculptEntry) + m_NoColide = true; // also don't colide if is a sculp or mesh and meshing is off if (!haveMesh) { -- cgit v1.1 From f740c9522aa5fd57ffd2d01fa9c2e244113ac880 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 27 May 2012 14:01:42 +0100 Subject: Let OOB information usable outside ubitode --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 29 +++++++++++++++++++++- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 23 ----------------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 10 ++++---- 3 files changed, 33 insertions(+), 29 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index fb90887..aaeae86 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -294,6 +294,34 @@ namespace OpenSim.Region.Physics.Manager public abstract Vector3 GeometricCenter { get; } public abstract Vector3 CenterOfMass { get; } + public virtual Vector3 OOBsize + { + get + { + Vector3 s=Size; + s.X *=0.5f; + s.Y *=0.5f; + s.Z *=0.5f; + return s; + } + } + + public virtual Vector3 OOBoffset + { + get + { + return Vector3.Zero; + } + } + + public virtual float OOBRadiusSQ + { + get + { + return Size.LengthSquared() * 0.25f; // ((0.5^2) + } + } + /// /// Velocity of this actor. /// @@ -429,7 +457,6 @@ namespace OpenSim.Region.Physics.Manager public override void VehicleFloatParam(int param, float value) { - } public override void VehicleVectorParam(int param, Vector3 value) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 1084b0e..6fb54cb 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -522,29 +522,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - //UBit mess - /* for later use - public override Vector3 PrimOOBsize - { - get - { - Vector3 s=Size; - s.X *=0.5f; - s.Y *=0.5f; - s.Z *=0.5f; - return s; - } - } - - public override Vector3 PrimOOBoffset - { - get - { - return Vector3.Zero; - } - } - */ - public override PrimitiveBaseShape Shape { set { return; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 496e7ac..1bfe08b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -504,8 +504,8 @@ namespace OpenSim.Region.Physics.OdePlugin } } } -/* - public override Vector3 PrimOOBsize + + public override Vector3 OOBsize { get { @@ -513,7 +513,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override Vector3 PrimOOBoffset + public override Vector3 OOBoffset { get { @@ -521,14 +521,14 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override float PrimOOBRadiusSQ + public override float OOBRadiusSQ { get { return primOOBradiusSQ; } } -*/ + public override PrimitiveBaseShape Shape { set -- cgit v1.1 From 20baa6334ca21ecd30ce1a46fcc4dbe5d4030854 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 28 May 2012 13:05:50 +0100 Subject: revert making sculpts phanton if sculpt meshing option is off --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 -- 1 file changed, 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 1bfe08b..5109a6a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1482,8 +1482,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (!haveMesh) m_NoColide = true; } - else if(_pbs.SculptEntry) - m_NoColide = true; // also don't colide if is a sculp or mesh and meshing is off if (!haveMesh) { -- cgit v1.1 From 60cebe9a5b91f2683019d645f6bb243a9f7db139 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 28 May 2012 14:32:16 +0100 Subject: let meshs work indenpendently of mesh_sculpted_prim config option --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 54 +++++++++++------------- 1 file changed, 25 insertions(+), 29 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index d5968fc..b0bc18b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1675,41 +1675,43 @@ namespace OpenSim.Region.Physics.OdePlugin /// public bool needsMeshing(PrimitiveBaseShape pbs) { - // most of this is redundant now as the mesher will return null if it cant mesh a prim - // but we still need to check for sculptie meshing being enabled so this is the most - // convenient place to do it for now... - - // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f) - // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString()); - int iPropertiesNotSupportedDefault = 0; + // check sculpts or meshs if (pbs.SculptEntry) { - if(!meshSculptedPrim) + if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs + return true; + + if (!meshSculptedPrim) return false; + else + return true; } + int iPropertiesNotSupportedDefault = 0; + + if (forceSimplePrimMeshing) + return true; + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim - if (!forceSimplePrimMeshing && !pbs.SculptEntry) - { - if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) + + if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) - { + { - if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 - && pbs.ProfileHollow == 0 - && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 - && pbs.PathBegin == 0 && pbs.PathEnd == 0 - && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 - && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 - && pbs.PathShearX == 0 && pbs.PathShearY == 0) - { + if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 + && pbs.ProfileHollow == 0 + && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 + && pbs.PathBegin == 0 && pbs.PathEnd == 0 + && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 + && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 + && pbs.PathShearX == 0 && pbs.PathShearY == 0) + { #if SPAM - m_log.Warn("NonMesh"); + m_log.Warn("NonMesh"); #endif - return false; - } + return false; } } @@ -1717,9 +1719,6 @@ namespace OpenSim.Region.Physics.OdePlugin // and it's odd.. so for now just return true if asked to force meshs // hopefully mesher will fail if doesn't suport so things still get basic boxes - if (forceSimplePrimMeshing) - return true; - if (pbs.ProfileHollow != 0) iPropertiesNotSupportedDefault++; @@ -1787,9 +1786,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - if (pbs.SculptEntry && meshSculptedPrim) - iPropertiesNotSupportedDefault++; - if (iPropertiesNotSupportedDefault == 0) { #if SPAM -- cgit v1.1 From 2122c336b088ebb2900cf6cd061602dda01268e0 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 28 May 2012 14:44:06 +0100 Subject: a bit cleaner code (?) on sculpts/meshs meshing checking --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index b0bc18b..286c7f0 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1675,20 +1675,17 @@ namespace OpenSim.Region.Physics.OdePlugin /// public bool needsMeshing(PrimitiveBaseShape pbs) { - // check sculpts or meshs if (pbs.SculptEntry) { - if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs + if (meshSculptedPrim) return true; - if (!meshSculptedPrim) - return false; - else + if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs return true; - } - int iPropertiesNotSupportedDefault = 0; + return false; + } if (forceSimplePrimMeshing) return true; @@ -1719,6 +1716,8 @@ namespace OpenSim.Region.Physics.OdePlugin // and it's odd.. so for now just return true if asked to force meshs // hopefully mesher will fail if doesn't suport so things still get basic boxes + int iPropertiesNotSupportedDefault = 0; + if (pbs.ProfileHollow != 0) iPropertiesNotSupportedDefault++; -- cgit v1.1 From dbbfe0cdd78c919b39da52e1ccc0c1fc500b57d5 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 28 May 2012 22:23:32 +0100 Subject: fix avatars collisions on sim crossings and other few cases where freemove() is called --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 6fb54cb..093bc3c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1322,8 +1322,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_iscollidingGround = false; m_iscollidingObj = false; - CollisionEventsThisFrame = new CollisionEventUpdate(); - m_eventsubscription = 0; + CollisionEventsThisFrame.Clear(); } private void changeForce(Vector3 newForce) -- cgit v1.1 From 3e9a831e87432d971a6006966e35924df1ed855d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 29 May 2012 19:13:27 +0100 Subject: fix physics not reporting collisions only with terrain --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 1 + 1 file changed, 1 insertion(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 093bc3c..6e4e41f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1118,6 +1118,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (CollisionEventsThisFrame == null) CollisionEventsThisFrame = new CollisionEventUpdate(); CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + _parent_scene.AddCollisionEventReporting(this); } public void SendCollisions() -- cgit v1.1 From 058707911551dd2f20740a636a8bb195623c2100 Mon Sep 17 00:00:00 2001 From: Melanie Date: Wed, 30 May 2012 19:25:52 +0200 Subject: Whitespace fix --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 5109a6a..628b727 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -4066,4 +4066,4 @@ namespace OpenSim.Region.Physics.OdePlugin public Vector3 value; } } -} \ No newline at end of file +} -- cgit v1.1 From c548f4879173ca63972eb09bcac76ca8b36e7108 Mon Sep 17 00:00:00 2001 From: Melanie Date: Wed, 30 May 2012 19:32:02 +0200 Subject: Guard setting the building flag --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 628b727..ac049b2 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -3320,6 +3320,10 @@ namespace OpenSim.Region.Physics.OdePlugin protected void changeBuilding(bool newbuilding) { + // Check if we need to do anything + if (newbuilding == m_building) + return; + if ((bool)newbuilding) { m_building = true; -- cgit v1.1 From cc903992dd3d74a7ff859f0e1a6363ac0c074520 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 30 May 2012 21:32:15 +0100 Subject: ubitode prim: if makebody() is called already having a body, do a full destrution so there are no 'leaks'. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index ac049b2..9a40ab5 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1621,12 +1621,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) { - d.BodyDestroy(Body); - Body = IntPtr.Zero; +// d.BodyDestroy(Body); +// Body = IntPtr.Zero; + // do a more complet destruction + DestroyBody(); m_log.Warn("[PHYSICS]: MakeBody called having a body"); } - if (d.GeomGetBody(prim_geom) != IntPtr.Zero) { d.GeomSetBody(prim_geom, IntPtr.Zero); -- cgit v1.1 From 401b97788f826397a552b25ab9b30c5549abd54f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 4 Jun 2012 20:35:12 +0100 Subject: fix addforce/impulse. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 9a40ab5..9b3b51b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -3252,9 +3252,9 @@ namespace OpenSim.Region.Physics.OdePlugin } - private void changeAddImpulse(Vector3 impulse) + private void changeAddForce(Vector3 theforce) { - m_forceacc += impulse *m_invTimeStep; + m_forceacc += theforce; if (!m_isSelected) { lock (this) @@ -3960,7 +3960,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; case changes.AddForce: - changeAddImpulse((Vector3)arg); + changeAddForce((Vector3)arg); break; case changes.AddAngForce: -- cgit v1.1 From ec7c7fe5f8b99a2aba4c2f9bc4913535e0e91507 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 30 Jun 2012 08:49:07 +0100 Subject: ubitode: create and use 4 off world collision spaces for offworld static prims --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 49 ++++++++++++++++++------ 1 file changed, 38 insertions(+), 11 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 286c7f0..6c72324 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -302,6 +302,7 @@ namespace OpenSim.Region.Physics.OdePlugin // split static geometry collision into a grid as before private IntPtr[,] staticPrimspace; + private IntPtr[] staticPrimspaceOffRegion; public Object OdeLock; private static Object SimulationLock; @@ -551,6 +552,7 @@ namespace OpenSim.Region.Physics.OdePlugin // create all spaces now int i, j; IntPtr newspace; + for (i = 0; i < spaceGridMaxX; i++) for (j = 0; j < spaceGridMaxY; j++) { @@ -573,6 +575,29 @@ namespace OpenSim.Region.Physics.OdePlugin // let this now be real maximum values spaceGridMaxX--; spaceGridMaxY--; + + // create 4 off world spaces (x<0,x>max,y<0,y>max) + staticPrimspaceOffRegion = new IntPtr[4]; + + for (i = 0; i < 4; i++) + { + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(newspace, 0); + + staticPrimspaceOffRegion[i] = newspace; + } + m_lastframe = DateTime.UtcNow; } @@ -1650,20 +1675,22 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr calculateSpaceForGeom(Vector3 pos) { int x, y; - x = (int)(pos.X * spacesPerMeter); - if (x < 0) - x = 0; - else if (x > spaceGridMaxX) - x = spaceGridMaxX; + if (pos.X < 0) + return staticPrimspaceOffRegion[0]; + + if (pos.Y < 0) + return staticPrimspaceOffRegion[2]; + + x = (int)(pos.X * spacesPerMeter); + if (x > spaceGridMaxX) + return staticPrimspaceOffRegion[1]; + y = (int)(pos.Y * spacesPerMeter); - if (y < 0) - y = 0; - else if (y >spaceGridMaxY) - y = spaceGridMaxY; + if (y > spaceGridMaxY) + return staticPrimspaceOffRegion[3]; - IntPtr tmpSpace = staticPrimspace[x, y]; - return tmpSpace; + return staticPrimspace[x, y]; } #endregion -- cgit v1.1 From d50b852d530bd17f2bb5a7964e9ddf81acff9146 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 7 Jul 2012 03:16:41 +0100 Subject: ODE turn off material dependent friction while vehicle linear motor is Effective. Increase a bit world damping of velocities --- OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs | 9 ++++++--- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 4 ++-- 3 files changed, 9 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index e27be1e..e900c02 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -137,6 +137,7 @@ namespace OpenSim.Region.Physics.OdePlugin float m_amdampY; float m_amdampZ; + public float FrictionFactor { get @@ -145,6 +146,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public ODEDynamics(OdePrim rootp) { rootPrim = rootp; @@ -345,7 +347,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; m_lmEfect = 1.0f; // turn it on - m_ffactor = 0.01f; + m_ffactor = 0.0f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) d.BodyEnable(rootPrim.Body); @@ -401,7 +403,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_lmEfect = 1.0f; // turn it on m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; - m_ffactor = 0.01f; + m_ffactor = 0.0f; if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) && !rootPrim.m_isSelected && !rootPrim.m_disabled) d.BodyEnable(rootPrim.Body); @@ -805,7 +807,8 @@ namespace OpenSim.Region.Physics.OdePlugin } m_lmEfect *= m_lmDecay; - m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); +// m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); + m_ffactor = 0.0f; } else { diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 9b3b51b..3d8e680 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -275,6 +275,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (veh != null && veh.Type != Vehicle.TYPE_NONE) cdata.mu *= veh.FrictionFactor; +// cdata.mu *= 0; } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 6c72324..4552f3f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -526,8 +526,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldSetGravity(world, gravityx, gravityy, gravityz); d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); - d.WorldSetLinearDamping(world, 0.001f); - d.WorldSetAngularDamping(world, 0.001f); + d.WorldSetLinearDamping(world, 0.002f); + d.WorldSetAngularDamping(world, 0.002f); d.WorldSetAngularDampingThreshold(world, 0f); d.WorldSetLinearDampingThreshold(world, 0f); d.WorldSetMaxAngularSpeed(world, 100f); -- cgit v1.1 From fb8e8dcbce6b2ecbb8fbfe8278657260ac55e823 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 9 Jul 2012 13:25:17 +0100 Subject: fix ODE dispose plus minor clean. On regions restart ode.dispose seems to be called with scene still calling simulation, that should be changed, for now added a check for a valid world in ode simulation --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 58 ++++++++++++++++++------ 1 file changed, 43 insertions(+), 15 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 4552f3f..5920838 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -268,7 +268,7 @@ namespace OpenSim.Region.Physics.OdePlugin public ContactData[] m_materialContactsData = new ContactData[8]; - private readonly DoubleDictionary RegionTerrain = new DoubleDictionary(); + private readonly Dictionary RegionTerrain = new Dictionary(); private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); private readonly Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); @@ -1892,18 +1892,22 @@ namespace OpenSim.Region.Physics.OdePlugin lock (SimulationLock) lock(OdeLock) { + if (world == IntPtr.Zero) + return 0; + // adjust number of iterations per step - try - { + +// try +// { d.WorldSetQuickStepNumIterations(world, curphysiteractions); - } +/* } catch (StackOverflowException) { m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); // ode.drelease(world); base.TriggerPhysicsBasedRestart(); } - +*/ while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever { try @@ -2383,11 +2387,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); - RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); -// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); + RegionTerrain.Add(pOffset, GroundGeom); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); - TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); - + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); } } @@ -2486,8 +2488,7 @@ namespace OpenSim.Region.Physics.OdePlugin geom_name_map[GroundGeom] = "Terrain"; d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); - RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); - // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); + RegionTerrain.Add(pOffset, GroundGeom); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); } @@ -2649,19 +2650,42 @@ namespace OpenSim.Region.Physics.OdePlugin public override void Dispose() { - m_rayCastManager.Dispose(); - m_rayCastManager = null; - lock (OdeLock) { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + lock (_prims) { + ChangesQueue.Clear(); foreach (OdePrim prm in _prims) { - RemovePrim(prm); + prm.DoAChange(changes.Remove, null); + _collisionEventPrim.Remove(prm); } + _prims.Clear(); } + OdeCharacter[] chtorem; + lock (_characters) + { + chtorem = new OdeCharacter[_characters.Count]; + _characters.CopyTo(chtorem); + } + + ChangesQueue.Clear(); + foreach (OdeCharacter ch in chtorem) + ch.DoAChange(changes.Remove, null); + + + foreach (IntPtr GroundGeom in RegionTerrain.Values) + { + if (GroundGeom != IntPtr.Zero) + d.GeomDestroy(GroundGeom); + } + + RegionTerrain.Clear(); + if (TerrainHeightFieldHeightsHandlers.Count > 0) { foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) @@ -2671,6 +2695,9 @@ namespace OpenSim.Region.Physics.OdePlugin } } + TerrainHeightFieldHeightsHandlers.Clear(); + TerrainHeightFieldHeights.Clear(); + if (WaterGeom != IntPtr.Zero) { d.GeomDestroy(WaterGeom); @@ -2691,6 +2718,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldDestroy(world); + world = IntPtr.Zero; //d.CloseODE(); } } -- cgit v1.1 From 0ac161c9a89075b414941d00b34b50e16c023a51 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 9 Jul 2012 16:51:56 +0100 Subject: log ODE lib configuration --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 5920838..46251de 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -175,7 +175,7 @@ namespace OpenSim.Region.Physics.OdePlugin const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; const float MaxERP = 0.8f; - const float minERP = 0.1f; + const float minERP = 0.2f; const float comumContactCFM = 0.0001f; float frictionMovementMult = 0.8f; @@ -408,7 +408,7 @@ namespace OpenSim.Region.Physics.OdePlugin // checkThread(); mesher = meshmerizer; m_config = config; -/* + string ode_config = d.GetConfiguration("ODE"); if (ode_config != null && ode_config != "") { @@ -419,7 +419,7 @@ namespace OpenSim.Region.Physics.OdePlugin OdeUbitLib = true; } } -*/ + /* if (region != null) { @@ -921,6 +921,8 @@ namespace OpenSim.Region.Physics.OdePlugin cfm = 0.0001f / cfm; if (cfm > 0.01f) cfm = 0.01f; + else if (cfm < 0.0001f) + cfm = 0.0001f; if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; @@ -947,6 +949,8 @@ namespace OpenSim.Region.Physics.OdePlugin cfm = 0.0001f / cfm; if (cfm > 0.01f) cfm = 0.01f; + else if (cfm < 0.0001f) + cfm = 0.0001f; if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) { -- cgit v1.1 From a7097680851528f01477903d946095abce99e504 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 9 Jul 2012 17:01:52 +0100 Subject: fix ode getconfiguration --- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 0e4961b..34865c1 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -1312,7 +1312,7 @@ namespace OdeAPI public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity] - public static extern string GetConfiguration(string str); + public static extern string GetConfiguration(); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] public static extern IntPtr HashSpaceCreate(IntPtr space); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 46251de..d5938f4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -409,7 +409,7 @@ namespace OpenSim.Region.Physics.OdePlugin mesher = meshmerizer; m_config = config; - string ode_config = d.GetConfiguration("ODE"); + string ode_config = d.GetConfiguration(); if (ode_config != null && ode_config != "") { m_log.WarnFormat("ODE configuration: {0}", ode_config); -- cgit v1.1 From 3a1d46ad446cb4d5d0f193bd9d7cdcfc4f65a565 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 9 Jul 2012 17:21:54 +0100 Subject: retry fixing ode getconfiguration() --- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 34865c1..2341186 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -1312,7 +1312,14 @@ namespace OdeAPI public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity] - public static extern string GetConfiguration(); + public static extern IntPtr iGetConfiguration(); + + public static string GetConfiguration() + { + IntPtr ptr = iGetConfiguration(); + string s = Marshal.PtrToStringAnsi(ptr); + return s; + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] public static extern IntPtr HashSpaceCreate(IntPtr space); -- cgit v1.1 From ca41ec9eb4616c0cb96ed48d591e473d95af2701 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 10 Jul 2012 05:11:06 +0100 Subject: let rotationVelocity or AngularVelocity be setted on prims. Limited to 12rad/s --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 65 +++++++++++++++++-------- 1 file changed, 46 insertions(+), 19 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 3d8e680..14e4272 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -583,8 +583,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { AddChange(changes.Velocity, value); -// _velocity = value; - } else { @@ -676,9 +674,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (value.IsFinite()) { - m_rotationalVelocity = value; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + AddChange(changes.AngVelocity, value); } else { @@ -687,7 +683,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override float Buoyancy { get { return m_buoyancy; } @@ -1737,17 +1732,14 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); -// d.BodySetLinearDampingThreshold(Body, 0.01f); -// d.BodySetAngularDampingThreshold(Body, 0.001f); - d.BodySetDamping(Body, .002f, .002f); - - if (m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - if (d.SpaceQuery(m_targetSpace, prim_geom)) - d.SpaceRemove(m_targetSpace, prim_geom); - } + d.BodySetDamping(Body, .005f, .005f); + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } if (childrenPrim.Count == 0) { @@ -3296,6 +3288,13 @@ namespace OpenSim.Region.Physics.OdePlugin private void changevelocity(Vector3 newVel) { + float len = newVel.LengthSquared(); + if (len > 100000.0f) // limit to 100m/s + { + len = 100.0f / (float)Math.Sqrt(len); + newVel *= len; + } + if (!m_isSelected) { if (Body != IntPtr.Zero) @@ -3312,6 +3311,33 @@ namespace OpenSim.Region.Physics.OdePlugin _velocity = newVel; } + + private void changeangvelocity(Vector3 newAngVel) + { + float len = newAngVel.LengthSquared(); + if (len > 144.0f) // limit to 12rad/s + { + len = 12.0f / (float)Math.Sqrt(len); + newAngVel *= len; + } + + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + + d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); + } + //resetCollisionAccounting(); + } + m_rotationalVelocity = newAngVel; + } + private void changeVolumedetetion(bool newVolDtc) { m_isVolumeDetect = newVolDtc; @@ -3948,9 +3974,10 @@ namespace OpenSim.Region.Physics.OdePlugin // case changes.Acceleration: // changeacceleration((Vector3)arg); // break; -// case changes.AngVelocity: -// changeangvelocity((Vector3)arg); -// break; + + case changes.AngVelocity: + changeangvelocity((Vector3)arg); + break; case changes.Force: changeForce((Vector3)arg); -- cgit v1.1 From 84ab4c44628441b32ec7ef0c728d035b5cf40b39 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 11 Jul 2012 08:13:57 +0100 Subject: ubitODE leaks --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 2 + OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 + OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 54 ++++++++++++---------- 3 files changed, 33 insertions(+), 25 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 6e4e41f..865180f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -740,6 +740,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Shell != IntPtr.Zero) { _parent_scene.geom_name_map.Remove(Shell); + _parent_scene.actor_name_map.Remove(Shell); _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); d.GeomDestroy(Shell); Shell = IntPtr.Zero; @@ -1188,6 +1189,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else { + _parent_scene.RemoveCollisionEventReporting(this); _parent_scene.RemoveCharacter(this); // destroy avatar capsule and related ODE data AvatarGeomAndBodyDestroy(); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 14e4272..ff17a6e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -943,6 +943,8 @@ namespace OpenSim.Region.Physics.OdePlugin CollisionEventsThisFrame = null; } m_eventsubscription = 0; + // for now still done on odescene +// _parent_scene.RemoveCollisionEventReporting(this); } public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index d5938f4..7848b35 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -175,7 +175,7 @@ namespace OpenSim.Region.Physics.OdePlugin const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; const float MaxERP = 0.8f; - const float minERP = 0.2f; + const float minERP = 0.1f; const float comumContactCFM = 0.0001f; float frictionMovementMult = 0.8f; @@ -237,20 +237,20 @@ namespace OpenSim.Region.Physics.OdePlugin private d.NearCallback nearCallback; - private readonly HashSet _characters = new HashSet(); - private readonly HashSet _prims = new HashSet(); - private readonly HashSet _activeprims = new HashSet(); - private readonly HashSet _activegroups = new HashSet(); + private HashSet _characters = new HashSet(); + private HashSet _prims = new HashSet(); + private HashSet _activeprims = new HashSet(); + private HashSet _activegroups = new HashSet(); public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); /// /// A list of actors that should receive collision events. /// - private readonly List _collisionEventPrim = new List(); - private readonly List _collisionEventPrimRemove = new List(); + private List _collisionEventPrim = new List(); + private List _collisionEventPrimRemove = new List(); - private readonly HashSet _badCharacter = new HashSet(); + private HashSet _badCharacter = new HashSet(); public Dictionary geom_name_map = new Dictionary(); public Dictionary actor_name_map = new Dictionary(); @@ -264,26 +264,21 @@ namespace OpenSim.Region.Physics.OdePlugin private volatile int m_global_contactcount = 0; - private readonly IntPtr contactgroup; + private IntPtr contactgroup; public ContactData[] m_materialContactsData = new ContactData[8]; - private readonly Dictionary RegionTerrain = new Dictionary(); - private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); - private readonly Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); + private Dictionary RegionTerrain = new Dictionary(); + private Dictionary TerrainHeightFieldHeights = new Dictionary(); + private Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); private int m_physicsiterations = 10; private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag - private readonly PhysicsActor PANull = new NullPhysicsActor(); + private PhysicsActor PANull = new NullPhysicsActor(); private float step_time = 0.0f; public IntPtr world; - private uint obj2LocalID = 0; - private OdeCharacter cc1; - private OdePrim cp1; - private OdeCharacter cc2; - private OdePrim cp2; // split the spaces acording to contents type // ActiveSpace contains characters and active prims @@ -921,8 +916,8 @@ namespace OpenSim.Region.Physics.OdePlugin cfm = 0.0001f / cfm; if (cfm > 0.01f) cfm = 0.01f; - else if (cfm < 0.0001f) - cfm = 0.0001f; + else if (cfm < 0.00001f) + cfm = 0.00001f; if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) mu *= frictionMovementMult; @@ -949,8 +944,8 @@ namespace OpenSim.Region.Physics.OdePlugin cfm = 0.0001f / cfm; if (cfm > 0.01f) cfm = 0.01f; - else if (cfm < 0.0001f) - cfm = 0.0001f; + else if (cfm < 0.00001f) + cfm = 0.00001f; if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) { @@ -993,6 +988,8 @@ namespace OpenSim.Region.Physics.OdePlugin cfm = 0.0001f / cfm; if (cfm > 0.01f) cfm = 0.01f; + else if (cfm < 0.00001f) + cfm = 0.00001f; if (curContact.side1 > 0) // should be 2 ? IgnoreNegSides = true; @@ -1159,7 +1156,13 @@ namespace OpenSim.Region.Physics.OdePlugin private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) { - obj2LocalID = 0; + + OdeCharacter cc1; + OdePrim cp1; + OdeCharacter cc2; + OdePrim cp2; + + uint obj2LocalID = 0; bool p1events = p1.SubscribedEvents(); bool p2events = p2.SubscribedEvents(); @@ -1963,6 +1966,7 @@ namespace OpenSim.Region.Physics.OdePlugin { RemoveCharacter(defect); } + defects.Clear(); } } @@ -2068,13 +2072,13 @@ namespace OpenSim.Region.Physics.OdePlugin _badCharacter.Clear(); } } - +/* int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); int nbodies = d.NTotalBodies; int ngeoms = d.NTotalGeoms; - +*/ // Finished with all sim stepping. If requested, dump world state to file for debugging. // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? -- cgit v1.1 From 4c2523b1c2dd518cc21baf7674775fa2e11e1ca9 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 12 Jul 2012 20:05:00 +0100 Subject: Use faster any contact point collision detection for Volumedetect, plus some clean up --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 123 +++++------------------ OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 2 + OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 14 ++- 3 files changed, 41 insertions(+), 98 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index ff17a6e..8d5f269 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -134,7 +134,7 @@ namespace OpenSim.Region.Physics.OdePlugin // private bool m_collidesLand = true; private bool m_collidesWater; - public bool m_returnCollisions; +// public bool m_returnCollisions; private bool m_NoColide; // for now only for internal use for bad meshs @@ -164,8 +164,8 @@ namespace OpenSim.Region.Physics.OdePlugin private List childrenPrim = new List(); - private bool m_throttleUpdates; - private int throttleCounter; +// private bool m_throttleUpdates; +// private int throttleCounter; public float m_collisionscore; int m_colliderfilter = 0; @@ -363,12 +363,14 @@ namespace OpenSim.Region.Physics.OdePlugin set { return; } } - public override bool ThrottleUpdates + + public override bool ThrottleUpdates {get;set;} +/* { get { return m_throttleUpdates; } set { m_throttleUpdates = value; } } - +*/ public override bool Stopped { get { return _zeroFlag; } @@ -951,7 +953,8 @@ namespace OpenSim.Region.Physics.OdePlugin { if (CollisionEventsThisFrame == null) CollisionEventsThisFrame = new CollisionEventUpdate(); - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); +// if(CollisionEventsThisFrame.Count < 32) + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); } public void SendCollisions() @@ -977,6 +980,10 @@ namespace OpenSim.Region.Physics.OdePlugin } else { + if (ncolisions > 10) + { + } + SentEmptyCollisionsEvent = false; CollisionEventsThisFrame.Clear(); } @@ -1746,8 +1753,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (childrenPrim.Count == 0) { collide_geom = prim_geom; - m_targetSpace = _parent_scene.ActiveSpace; - d.SpaceAdd(m_targetSpace, prim_geom); + m_targetSpace = _parent_scene.ActiveSpace; } else { @@ -1755,7 +1761,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.HashSpaceSetLevels(m_targetSpace, -2, 8); d.SpaceSetSublevel(m_targetSpace, 3); d.SpaceSetCleanup(m_targetSpace, false); - d.SpaceAdd(m_targetSpace, prim_geom); d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | CollisionCategories.Geom | @@ -1766,12 +1771,21 @@ namespace OpenSim.Region.Physics.OdePlugin collide_geom = m_targetSpace; } + d.SpaceAdd(m_targetSpace, prim_geom); + if (m_delaySelect) { m_isSelected = true; m_delaySelect = false; } + m_collisionscore = 0; + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + + _parent_scene.addActivePrim(this); + lock (childrenPrim) { foreach (OdePrim prm in childrenPrim) @@ -1809,10 +1823,6 @@ namespace OpenSim.Region.Physics.OdePlugin createAMotor(m_angularlock); } - m_collisionscore = 0; - - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); if (m_isSelected || m_disabled) { @@ -1824,7 +1834,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); } - _parent_scene.addActivePrim(this); _parent_scene.addActiveGroups(this); } @@ -3441,92 +3450,14 @@ namespace OpenSim.Region.Physics.OdePlugin if (++bodydisablecontrol < 20) return; - bodydisablecontrol = 0; + d.BodyEnable(Body); } - d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - -/* moved down to UpdateMove... where it belongs again - - // check outside region - - if (lpos.Z < -100 || lpos.Z > 100000f) - { - m_outbounds = true; - - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - - throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - return; - } - - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } + bodydisablecontrol = 0; - if (m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; - - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; + d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - base.RequestPhysicsterseUpdate(); - return; - } -*/ if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) { // 'VEHICLES' are dealt with in ODEDynamics.cs @@ -3721,7 +3652,7 @@ namespace OpenSim.Region.Physics.OdePlugin base.RequestPhysicsterseUpdate(); - throttleCounter = 0; +// throttleCounter = 0; _zeroFlag = true; disableBodySoft(); // disable it and colisions diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 2341186..ee48db5 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -61,6 +61,8 @@ namespace OdeAPI public static int NTotalBodies = 0; public static int NTotalGeoms = 0; + public const uint CONTACTS_UNIMPORTANT = 0x80000000; + #region Flags and Enumerations [Flags] diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 7848b35..088d2ba 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -713,8 +713,18 @@ namespace OpenSim.Region.Physics.OdePlugin if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; - - count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || + d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) + { + int cflags; + unchecked + { + cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); + } + count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + else + count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); } catch (SEHException) { -- cgit v1.1 From 5cfea5934ba2aa19bc864c950c60a4c29f017316 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 13 Jul 2012 19:32:43 +0100 Subject: fix bad vehicle reference frame --- OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index e900c02..a7dda7a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -425,7 +425,8 @@ namespace OpenSim.Region.Physics.OdePlugin switch (pParam) { case Vehicle.REFERENCE_FRAME: - m_referenceFrame = Quaternion.Inverse(pValue); + // m_referenceFrame = Quaternion.Inverse(pValue); + m_referenceFrame = pValue; break; case Vehicle.ROLL_FRAME: m_RollreferenceFrame = pValue; -- cgit v1.1 From 72e2b9409462861d183ac7b391f5911defcd3bb0 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 13 Jul 2012 23:57:45 +0100 Subject: In collisions report linksets root parts to parts, and not all parts. Temporary suspend collision checks on full stopped bodies, until a better away is found wake them, avoiding spurius collision end and start events. Until a nice way is found to avoid them, this may cause some higher cpu load. plus some clean up --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 39 ++++++++++----------- .../UbitOdePlugin/ODERayCastRequestManager.cs | 4 +-- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 40 +++++++++++++--------- 4 files changed, 45 insertions(+), 41 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 865180f..b506b1c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -95,7 +95,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_iscollidingObj = false; private bool m_alwaysRun = false; private int m_requestedUpdateFrequency = 0; - public uint m_localID = 0; + private uint m_localID = 0; public bool m_returnCollisions = false; // taints and their non-tainted counterparts public bool m_isPhysical = false; // the current physical status @@ -214,6 +214,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override uint LocalID { + get { return m_localID; } set { m_localID = value; } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 8d5f269..6d322e2 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -101,7 +101,6 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_invTimeStep = 50.0f; private float m_timeStep = .02f; - private Vector3 m_PIDTarget; private float m_PIDTau; private bool m_usePID; @@ -119,7 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. private int body_autodisable_frames = 5; - private int bodydisablecontrol = 0; + public int bodydisablecontrol = 0; // Default we're a Geometry @@ -144,7 +143,7 @@ namespace OpenSim.Region.Physics.OdePlugin public bool m_disabled; - public uint m_localID; + private uint m_localID; private IMesh m_mesh; private object m_meshlock = new object(); @@ -167,7 +166,7 @@ namespace OpenSim.Region.Physics.OdePlugin // private bool m_throttleUpdates; // private int throttleCounter; public float m_collisionscore; - int m_colliderfilter = 0; + private int m_colliderfilter = 0; public IntPtr collide_geom; // for objects: geom if single prim space it linkset @@ -235,7 +234,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override bool Phantom // this is not reliable for internal use { get { return m_fakeisphantom; } @@ -293,14 +291,18 @@ namespace OpenSim.Region.Physics.OdePlugin public override uint LocalID { + get { return m_localID; } + set { m_localID = value; } + } + + public OdePrim Parent + { get { - return m_localID; - } - set - { - //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); - m_localID = value; + if (childPrim) + return (OdePrim)_parent; + else + return this; } } @@ -945,8 +947,7 @@ namespace OpenSim.Region.Physics.OdePlugin CollisionEventsThisFrame = null; } m_eventsubscription = 0; - // for now still done on odescene -// _parent_scene.RemoveCollisionEventReporting(this); + _parent_scene.RemoveCollisionEventReporting(this); } public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) @@ -980,10 +981,6 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - if (ncolisions > 10) - { - } - SentEmptyCollisionsEvent = false; CollisionEventsThisFrame.Clear(); } @@ -1832,8 +1829,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - } - + } _parent_scene.addActiveGroups(this); } @@ -3700,6 +3696,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAngularVel(Body, 0, 0, 0); d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); disableBodySoft(); // stop collisions + UnSubscribeEvents(); + base.RequestPhysicsterseUpdate(); return; } @@ -3871,8 +3869,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_vehicle = null; RemoveGeom(); m_targetSpace = IntPtr.Zero; - if (m_eventsubscription > 0) - UnSubscribeEvents(); + UnSubscribeEvents(); return true; case changes.Link: diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 5122ebf..3d108f8 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -509,14 +509,14 @@ namespace OpenSim.Region.Physics.OdePlugin if ((thisFlags & CurrentRayFilter) == 0) return; - ID = ((OdePrim)p2).m_localID; + ID = ((OdePrim)p2).LocalID; } else if (p2 is OdeCharacter) { if ((CurrentRayFilter & RayFilterFlags.agent) == 0) return; else - ID = ((OdeCharacter)p2).m_localID; + ID = ((OdeCharacter)p2).LocalID; } else //?? return; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 088d2ba..659180d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1171,6 +1171,8 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim cp1; OdeCharacter cc2; OdePrim cp2; + OdePrim cp1Parent; + OdePrim cp2Parent; uint obj2LocalID = 0; bool p1events = p1.SubscribedEvents(); @@ -1207,18 +1209,19 @@ namespace OpenSim.Region.Physics.OdePlugin { case ActorTypes.Agent: cc2 = (OdeCharacter)p2; - obj2LocalID = cc2.m_localID; + obj2LocalID = cc2.LocalID; if (p2events) - cc2.AddCollisionEvent(cc1.m_localID, contact); + cc2.AddCollisionEvent(cc1.LocalID, contact); break; case ActorTypes.Prim: if (p2 is OdePrim) { cp2 = (OdePrim)p2; - obj2LocalID = cp2.m_localID; if (p2events) - cp2.AddCollisionEvent(cc1.m_localID, contact); + cp2.AddCollisionEvent(cc1.LocalID, contact); + cp2 = cp2.Parent; + obj2LocalID = cp2.LocalID; } break; @@ -1240,17 +1243,16 @@ namespace OpenSim.Region.Physics.OdePlugin if (p1 is OdePrim) { cp1 = (OdePrim)p1; - - // obj1LocalID = cp2.m_localID; + cp1Parent = cp1.Parent; switch ((ActorTypes)p2.PhysicsActorType) { case ActorTypes.Agent: if (p2 is OdeCharacter) { cc2 = (OdeCharacter)p2; - obj2LocalID = cc2.m_localID; + obj2LocalID = cc2.LocalID; if (p2events) - cc2.AddCollisionEvent(cp1.m_localID, contact); + cc2.AddCollisionEvent(cp1Parent.LocalID, contact); } break; case ActorTypes.Prim: @@ -1258,9 +1260,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (p2 is OdePrim) { cp2 = (OdePrim)p2; - obj2LocalID = cp2.m_localID; if (p2events) - cp2.AddCollisionEvent(cp1.m_localID, contact); + cp2.AddCollisionEvent(cp1Parent.LocalID, contact); + cp2 = cp2.Parent; + obj2LocalID = cp2.LocalID; } break; @@ -1286,7 +1289,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (p2 is OdeCharacter) { cc2 = (OdeCharacter)p2; - obj2LocalID = cc2.m_localID; + obj2LocalID = cc2.LocalID; if (p2events) cc2.AddCollisionEvent(0, contact); } @@ -1295,7 +1298,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (p2 is OdePrim) { cp2 = (OdePrim)p2; - obj2LocalID = cp2.m_localID; + obj2LocalID = cp2.LocalID; if (p2events) cp2.AddCollisionEvent(0, contact); } @@ -1350,8 +1353,11 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdePrim prm in _activegroups) { - if (d.BodyIsEnabled(prm.Body) && !prm.m_outbounds) - d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); + if (!prm.m_outbounds) + { +// if (d.BodyIsEnabled(prm.Body)) + d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); + } } } catch (AccessViolationException) @@ -1604,7 +1610,7 @@ namespace OpenSim.Region.Physics.OdePlugin //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); lock (prim) { - RemoveCollisionEventReporting(prim); +// RemoveCollisionEventReporting(prim); lock (_prims) _prims.Remove(prim); } @@ -2012,6 +2018,7 @@ namespace OpenSim.Region.Physics.OdePlugin case ActorTypes.Prim: OdePrim pobj = (OdePrim)obj; if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) + if (!pobj.m_outbounds) { pobj.AddCollisionFrameTime((int)(odetimestepMS)); pobj.SendCollisions(); @@ -2728,7 +2735,6 @@ namespace OpenSim.Region.Physics.OdePlugin WaterMapHandler.Free(); } - if (ContactgeomsArray != IntPtr.Zero) Marshal.FreeHGlobal(ContactgeomsArray); if (GlobalContactsArray != IntPtr.Zero) @@ -2751,7 +2757,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prm.CollisionScore > 0) { - returncolliders.Add(prm.m_localID, prm.CollisionScore); + returncolliders.Add(prm.LocalID, prm.CollisionScore); cnt++; prm.CollisionScore = 0f; if (cnt > 25) -- cgit v1.1 From 95f2d8654989807c1bc4fe39cd8715210ac1a9ae Mon Sep 17 00:00:00 2001 From: Melanie Date: Sat, 14 Jul 2012 09:13:27 +0200 Subject: Reinstate sleeping because less CPU is preferred over reporting accuracy. Who needs accurate collisions when the sim lags and crashes? --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 659180d..2928257 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1355,7 +1355,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (!prm.m_outbounds) { -// if (d.BodyIsEnabled(prm.Body)) + if (d.BodyIsEnabled(prm.Body)) d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); } } -- cgit v1.1 From 62df82b74d0f3599585f7320aeab8c6a8262f61f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 15 Jul 2012 00:50:00 +0100 Subject: messing around... Let terrain and water have nullphysicsactors, let nullphyscisactors have a type water, ground or unknown (default). having this removed geom to name mapping no longer needed. Made some more methods comum to prims and characters acessible via PhysActor allowing for a more uniform access. ... --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 143 +++----- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 16 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 14 +- .../UbitOdePlugin/ODERayCastRequestManager.cs | 99 +++--- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 6 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 378 ++++++++------------- 6 files changed, 268 insertions(+), 388 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index aaeae86..a2c72c3 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -43,7 +43,8 @@ namespace OpenSim.Region.Physics.Manager Unknown = 0, Agent = 1, Prim = 2, - Ground = 3 + Ground = 3, + Water = 4 } public enum PIDHoverType @@ -114,7 +115,7 @@ namespace OpenSim.Region.Physics.Manager m_objCollisionList.Add(localID, contact); } else - { + { if (m_objCollisionList[localID].PenetrationDepth < contact.PenetrationDepth) m_objCollisionList[localID] = contact; } @@ -202,7 +203,7 @@ namespace OpenSim.Region.Physics.Manager /// XXX: Bizarrely, this cannot be "Terrain" or "Water" right now unless it really is simulating terrain or /// water. This is not a problem due to the formatting of names given by prims and avatars. /// - public string Name { get; protected set; } + public string Name { get; set; } /// /// This is being used by ODE joint code. @@ -230,11 +231,6 @@ namespace OpenSim.Region.Physics.Manager } } - public virtual byte[] Serialize(bool PhysIsRunning) - { - return new byte[0]; - } - public virtual void RaiseOutOfBounds(Vector3 pos) { // Make a temporary copy of the event to avoid possibility of @@ -258,10 +254,7 @@ namespace OpenSim.Region.Physics.Manager handler(e); } - public virtual void SetMaterial (int material) - { - } - + public virtual void SetMaterial (int material) { } public virtual float Density { get; set; } public virtual float GravModifier { get; set; } public virtual float Friction { get; set; } @@ -373,13 +366,21 @@ namespace OpenSim.Region.Physics.Manager public abstract void SubscribeEvents(int ms); public abstract void UnSubscribeEvents(); public abstract bool SubscribedEvents(); + + public virtual void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { } + + // Warning in a parent part it returns itself, not null + public virtual PhysicsActor ParentActor { get { return this; } } + } public class NullPhysicsActor : PhysicsActor { + private ActorTypes m_actorType = ActorTypes.Unknown; + public override bool Stopped { - get{ return false; } + get{ return true; } } public override Vector3 Position @@ -396,6 +397,7 @@ namespace OpenSim.Region.Physics.Manager public override uint LocalID { + get { return 0; } set { return; } } @@ -455,49 +457,17 @@ namespace OpenSim.Region.Physics.Manager set { return; } } - public override void VehicleFloatParam(int param, float value) - { - } - - public override void VehicleVectorParam(int param, Vector3 value) - { - - } - - public override void VehicleRotationParam(int param, Quaternion rotation) - { - - } - - public override void VehicleFlags(int param, bool remove) - { - - } - - public override void SetVolumeDetect(int param) - { - - } - - public override void SetMaterial(int material) - { - - } - - public override Vector3 CenterOfMass - { - get { return Vector3.Zero; } - } + public override void VehicleFloatParam(int param, float value) {} + public override void VehicleVectorParam(int param, Vector3 value) { } + public override void VehicleRotationParam(int param, Quaternion rotation) { } + public override void VehicleFlags(int param, bool remove) { } + public override void SetVolumeDetect(int param) {} + public override void SetMaterial(int material) {} + public override Vector3 CenterOfMass { get { return Vector3.Zero; }} - public override Vector3 GeometricCenter - { - get { return Vector3.Zero; } - } + public override Vector3 GeometricCenter { get { return Vector3.Zero; }} - public override PrimitiveBaseShape Shape - { - set { return; } - } + public override PrimitiveBaseShape Shape { set { return; }} public override Vector3 Velocity { @@ -517,9 +487,7 @@ namespace OpenSim.Region.Physics.Manager set { } } - public override void CrossingFailure() - { - } + public override void CrossingFailure() {} public override Quaternion Orientation { @@ -559,8 +527,20 @@ namespace OpenSim.Region.Physics.Manager public override int PhysicsActorType { - get { return (int) ActorTypes.Unknown; } - set { return; } + get { return (int)m_actorType; } + set { + ActorTypes type = (ActorTypes)value; + switch (type) + { + case ActorTypes.Ground: + case ActorTypes.Water: + m_actorType = type; + break; + default: + m_actorType = ActorTypes.Unknown; + break; + } + } } public override bool Kinematic @@ -569,26 +549,11 @@ namespace OpenSim.Region.Physics.Manager set { return; } } - public override void link(PhysicsActor obj) - { - } - - public override void delink() - { - } - - public override void LockAngularMotion(Vector3 axis) - { - } - - public override void AddForce(Vector3 force, bool pushforce) - { - } - - public override void AddAngularForce(Vector3 force, bool pushforce) - { - - } + public override void link(PhysicsActor obj) { } + public override void delink() { } + public override void LockAngularMotion(Vector3 axis) { } + public override void AddForce(Vector3 force, bool pushforce) { } + public override void AddAngularForce(Vector3 force, bool pushforce) { } public override Vector3 RotationalVelocity { @@ -610,22 +575,10 @@ namespace OpenSim.Region.Physics.Manager public override float APIDStrength { set { return; } } public override float APIDDamping { set { return; } } - public override void SetMomentum(Vector3 momentum) - { - } - - public override void SubscribeEvents(int ms) - { - - } - public override void UnSubscribeEvents() - { - - } - public override bool SubscribedEvents() - { - return false; - } + public override void SetMomentum(Vector3 momentum) { } + public override void SubscribeEvents(int ms) { } + public override void UnSubscribeEvents() { } + public override bool SubscribedEvents() { return false; } } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b506b1c..c363310 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -106,7 +106,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_freemove = false; // private CollisionLocker ode; - private string m_name = String.Empty; +// private string m_name = String.Empty; // other filter control int m_colliderfilter = 0; int m_colliderGroundfilter = 0; @@ -183,7 +183,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_isPhysical = false; // current status: no ODE information exists - m_name = avName; + Name = avName; AddChange(changes.Add, null); } @@ -218,6 +218,11 @@ namespace OpenSim.Region.Physics.OdePlugin set { m_localID = value; } } + public override PhysicsActor ParentActor + { + get { return (PhysicsActor)this; } + } + public override bool Grabbed { set { return; } @@ -740,7 +745,7 @@ namespace OpenSim.Region.Physics.OdePlugin //kill the Geometry if (Shell != IntPtr.Zero) { - _parent_scene.geom_name_map.Remove(Shell); +// _parent_scene.geom_name_map.Remove(Shell); _parent_scene.actor_name_map.Remove(Shell); _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); d.GeomDestroy(Shell); @@ -1115,7 +1120,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_eventsubscription = 0; } - public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { if (CollisionEventsThisFrame == null) CollisionEventsThisFrame = new CollisionEventUpdate(); @@ -1184,7 +1189,7 @@ namespace OpenSim.Region.Physics.OdePlugin } AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); - _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; _parent_scene.AddCharacter(this); } @@ -1236,7 +1241,6 @@ namespace OpenSim.Region.Physics.OdePlugin Velocity = Vector3.Zero; - _parent_scene.geom_name_map[Shell] = m_name; _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; } else diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 6d322e2..6bf5be1 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -176,7 +176,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_lastUpdateSent; public IntPtr Body = IntPtr.Zero; - public String Name { get; private set; } +// public String Name { get; private set; } private Vector3 _target_velocity; public Vector3 primOOBsize; // prim real dimensions from mesh @@ -295,14 +295,14 @@ namespace OpenSim.Region.Physics.OdePlugin set { m_localID = value; } } - public OdePrim Parent + public override PhysicsActor ParentActor { get { if (childPrim) - return (OdePrim)_parent; + return _parent; else - return this; + return (PhysicsActor)this; } } @@ -950,7 +950,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.RemoveCollisionEventReporting(this); } - public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { if (CollisionEventsThisFrame == null) CollisionEventsThisFrame = new CollisionEventUpdate(); @@ -1431,6 +1431,7 @@ namespace OpenSim.Region.Physics.OdePlugin //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); if (prim_geom != IntPtr.Zero) { + if (m_NoColide) { d.GeomSetCategoryBits(prim_geom, 0); @@ -1452,7 +1453,6 @@ namespace OpenSim.Region.Physics.OdePlugin CalcPrimBodyData(); - _parent_scene.geom_name_map[prim_geom] = Name; _parent_scene.actor_name_map[prim_geom] = this; } @@ -1526,7 +1526,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prim_geom != IntPtr.Zero) { - _parent_scene.geom_name_map.Remove(prim_geom); +// _parent_scene.geom_name_map.Remove(prim_geom); _parent_scene.actor_name_map.Remove(prim_geom); try { diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 3d108f8..21fe9c0 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -470,56 +470,77 @@ namespace OpenSim.Region.Physics.OdePlugin if (p2 == null) { - string name; - - if (!m_scene.geom_name_map.TryGetValue(g2, out name)) - return; - - if (name == "Terrain") - { - // land colision - if ((CurrentRayFilter & RayFilterFlags.land) == 0) - return; - } - else if (name == "Water") - { - if ((CurrentRayFilter & RayFilterFlags.water) == 0) - return; - } - else - return; + /* + string name; + + if (!m_scene.geom_name_map.TryGetValue(g2, out name)) + return; + + if (name == "Terrain") + { + // land colision + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + } + else if (name == "Water") + { + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + } + else + return; + */ + return; } else { - if (p2 is OdePrim) + switch (p2.PhysicsActorType) { - RayFilterFlags thisFlags; + case (int)ActorTypes.Prim: - if (p2.IsPhysical) - thisFlags = RayFilterFlags.physical; - else - thisFlags = RayFilterFlags.nonphysical; + RayFilterFlags thisFlags; - if (p2.Phantom) - thisFlags |= RayFilterFlags.phantom; + if (p2.IsPhysical) + thisFlags = RayFilterFlags.physical; + else + thisFlags = RayFilterFlags.nonphysical; - if (p2.IsVolumeDtc) - thisFlags |= RayFilterFlags.volumedtc; + if (p2.Phantom) + thisFlags |= RayFilterFlags.phantom; - if ((thisFlags & CurrentRayFilter) == 0) - return; + if (p2.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; - ID = ((OdePrim)p2).LocalID; - } - else if (p2 is OdeCharacter) - { - if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + if ((thisFlags & CurrentRayFilter) == 0) + return; + + ID = ((OdePrim)p2).LocalID; + break; + + case (int)ActorTypes.Agent: + + if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + return; + else + ID = ((OdeCharacter)p2).LocalID; + break; + + case (int)ActorTypes.Ground: + + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + break; + + case (int)ActorTypes.Water: + + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + break; + + default: return; - else - ID = ((OdeCharacter)p2).LocalID; + break; } - else //?? - return; } d.ContactGeom curcontact = new d.ContactGeom(); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index ee48db5..403a4ce 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -903,7 +903,7 @@ namespace OdeAPI public static extern GeomClassID GeomGetClass(IntPtr geom); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomGetData(IntPtr geom); + public static extern IntPtr GeomGetData(IntPtr geom); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); @@ -1096,8 +1096,8 @@ namespace OdeAPI [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetData(IntPtr geom, IntPtr data); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetData(IntPtr geom, IntPtr data); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity] public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 659180d..1104f45 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -251,7 +251,7 @@ namespace OpenSim.Region.Physics.OdePlugin private List _collisionEventPrimRemove = new List(); private HashSet _badCharacter = new HashSet(); - public Dictionary geom_name_map = new Dictionary(); +// public Dictionary geom_name_map = new Dictionary(); public Dictionary actor_name_map = new Dictionary(); private float contactsurfacelayer = 0.002f; @@ -274,7 +274,7 @@ namespace OpenSim.Region.Physics.OdePlugin private int m_physicsiterations = 10; private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag - private PhysicsActor PANull = new NullPhysicsActor(); +// private PhysicsActor PANull = new NullPhysicsActor(); private float step_time = 0.0f; public IntPtr world; @@ -713,6 +713,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; + if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) { @@ -738,7 +739,7 @@ namespace OpenSim.Region.Physics.OdePlugin return; } - // id contacts done + // contacts done if (count == 0) return; @@ -748,12 +749,14 @@ namespace OpenSim.Region.Physics.OdePlugin if (!actor_name_map.TryGetValue(g1, out p1)) { - p1 = PANull; + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); + return; } if (!actor_name_map.TryGetValue(g2, out p2)) { - p2 = PANull; + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + return; } // update actors collision score @@ -765,7 +768,6 @@ namespace OpenSim.Region.Physics.OdePlugin p2.CollisionScore = 0; p2.CollisionScore += count; - // get first contact d.ContactGeom curContact = new d.ContactGeom(); if (!GetCurContactGeom(0, ref curContact)) @@ -798,7 +800,6 @@ namespace OpenSim.Region.Physics.OdePlugin ContactData contactdata1 = new ContactData(0, 0, false); ContactData contactdata2 = new ContactData(0, 0, false); - String name = null; bool dop1foot = false; bool dop2foot = false; bool ignore = false; @@ -830,34 +831,16 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: -/* - p1.getContactData(ref contactdata1); - p2.getContactData(ref contactdata2); - - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - - if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) - mu *= frictionMovementMult; -*/ p1.CollidingObj = true; p2.CollidingObj = true; break; - case (int)ActorTypes.Prim: -/* - p1.getContactData(ref contactdata1); - p2.getContactData(ref contactdata2); - - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - - if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) - mu *= frictionMovementMult; - */ + case (int)ActorTypes.Prim: if (p2.Velocity.LengthSquared() > 0.0f) p2.CollidingObj = true; - dop1foot = true; break; + default: ignore = true; // avatar to terrain and water ignored break; @@ -869,9 +852,6 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: - // p1.getContactData(ref contactdata1); - // p2.getContactData(ref contactdata2); - AvanormOverride = true; Vector3 tmp = p2.Position - p1.Position; @@ -894,16 +874,12 @@ namespace OpenSim.Region.Physics.OdePlugin bounce = 0; mu = 0; cfm = 0.0001f; - /* - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) - mu *= frictionMovementMult; - */ dop2foot = true; if (p1.Velocity.LengthSquared() > 0.0f) p1.CollidingObj = true; break; + case (int)ActorTypes.Prim: if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) { @@ -933,95 +909,77 @@ namespace OpenSim.Region.Physics.OdePlugin mu *= frictionMovementMult; break; - default: - if (geom_name_map.TryGetValue(g2, out name)) - { - if (name == "Terrain") - { - p1.getContactData(ref contactdata1); - bounce = contactdata1.bounce * TerrainBounce; - mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); - if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) - mu *= frictionMovementMult; - p1.CollidingGround = true; - - cfm = p1.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - if (dscale > 1.0f) - dscale = 1.0f; - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; - else if (cfm < 0.00001f) - cfm = 0.00001f; - - if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) - { - if (curContact.side1 > 0) - IgnoreNegSides = true; - } - - } - else if (name == "Water") - { - ignore = true; - } - } - else - ignore = true; - break; - } - break; - - default: - if (geom_name_map.TryGetValue(g1, out name)) - { - if (name == "Terrain") - { - if (p2.PhysicsActorType == (int)ActorTypes.Prim) - { - p2.CollidingGround = true; - p2.getContactData(ref contactdata2); - bounce = contactdata2.bounce * TerrainBounce; - mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); - - cfm = p2.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - if (dscale > 1.0f) - dscale = 1.0f; + case (int)ActorTypes.Ground: + p1.getContactData(ref contactdata1); + bounce = contactdata1.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + p1.CollidingGround = true; - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; - else if (cfm < 0.00001f) - cfm = 0.00001f; + cfm = p1.Mass; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + if (dscale > 1.0f) + dscale = 1.0f; + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + else if (cfm < 0.00001f) + cfm = 0.00001f; - if (curContact.side1 > 0) // should be 2 ? + if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) + { + if (curContact.side1 > 0) IgnoreNegSides = true; - - if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) - mu *= frictionMovementMult; } - else - ignore = true; + break; - } - else if (name == "Water" && - (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent)) - { + case (int)ActorTypes.Water: + default: ignore = true; - } + break; + } + break; + + case (int)ActorTypes.Ground: + if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + p2.CollidingGround = true; + p2.getContactData(ref contactdata2); + bounce = contactdata2.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + + cfm = p2.Mass; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + + if (dscale > 1.0f) + dscale = 1.0f; + + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + else if (cfm < 0.00001f) + cfm = 0.00001f; + + if (curContact.side1 > 0) // should be 2 ? + IgnoreNegSides = true; + + if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; } else ignore = true; break; - } + case (int)ActorTypes.Water: + default: + break; + } if (ignore) return; @@ -1162,36 +1120,23 @@ namespace OpenSim.Region.Physics.OdePlugin } */ - } + } private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) - { - - OdeCharacter cc1; - OdePrim cp1; - OdeCharacter cc2; - OdePrim cp2; - OdePrim cp1Parent; - OdePrim cp2Parent; - + { uint obj2LocalID = 0; + bool p1events = p1.SubscribedEvents(); bool p2events = p2.SubscribedEvents(); - + if (p1.IsVolumeDtc) p2events = false; if (p2.IsVolumeDtc) p1events = false; - if (!(p2events || p1events)) + if (!p2events && !p1events) return; - if (p1events) - AddCollisionEventReporting(p1); - - if (p2events) - AddCollisionEventReporting(p2); - Vector3 vel = Vector3.Zero; if (p2 != null && p2.IsPhysical) vel = p2.Velocity; @@ -1200,71 +1145,22 @@ namespace OpenSim.Region.Physics.OdePlugin vel -= p1.Velocity; contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); - + switch ((ActorTypes)p1.PhysicsActorType) - { + { case ActorTypes.Agent: - cc1 = (OdeCharacter)p1; - switch ((ActorTypes)p2.PhysicsActorType) - { - case ActorTypes.Agent: - cc2 = (OdeCharacter)p2; - obj2LocalID = cc2.LocalID; - if (p2events) - cc2.AddCollisionEvent(cc1.LocalID, contact); - break; - - case ActorTypes.Prim: - if (p2 is OdePrim) - { - cp2 = (OdePrim)p2; - if (p2events) - cp2.AddCollisionEvent(cc1.LocalID, contact); - cp2 = cp2.Parent; - obj2LocalID = cp2.LocalID; - } - break; - - case ActorTypes.Ground: - case ActorTypes.Unknown: - default: - obj2LocalID = 0; - break; - } - if (p1events) - { - contact.SurfaceNormal = -contact.SurfaceNormal; - cc1.AddCollisionEvent(obj2LocalID, contact); - } - break; - case ActorTypes.Prim: - - if (p1 is OdePrim) - { - cp1 = (OdePrim)p1; - cp1Parent = cp1.Parent; + { switch ((ActorTypes)p2.PhysicsActorType) - { + { case ActorTypes.Agent: - if (p2 is OdeCharacter) - { - cc2 = (OdeCharacter)p2; - obj2LocalID = cc2.LocalID; - if (p2events) - cc2.AddCollisionEvent(cp1Parent.LocalID, contact); - } - break; case ActorTypes.Prim: - - if (p2 is OdePrim) - { - cp2 = (OdePrim)p2; - if (p2events) - cp2.AddCollisionEvent(cp1Parent.LocalID, contact); - cp2 = cp2.Parent; - obj2LocalID = cp2.LocalID; - } + if (p2events) + { + AddCollisionEventReporting(p2); + p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); + } + obj2LocalID = p2.ParentActor.LocalID; break; case ActorTypes.Ground: @@ -1272,41 +1168,28 @@ namespace OpenSim.Region.Physics.OdePlugin default: obj2LocalID = 0; break; - } + } if (p1events) - { + { contact.SurfaceNormal = -contact.SurfaceNormal; - cp1.AddCollisionEvent(obj2LocalID, contact); - } + AddCollisionEventReporting(p1); + p1.AddCollisionEvent(obj2LocalID, contact); } - break; + break; + } case ActorTypes.Ground: case ActorTypes.Unknown: default: - switch ((ActorTypes)p2.PhysicsActorType) + { + if (p2events && !p2.IsVolumeDtc) { - case ActorTypes.Agent: - if (p2 is OdeCharacter) - { - cc2 = (OdeCharacter)p2; - obj2LocalID = cc2.LocalID; - if (p2events) - cc2.AddCollisionEvent(0, contact); - } - break; - case ActorTypes.Prim: - if (p2 is OdePrim) - { - cp2 = (OdePrim)p2; - obj2LocalID = cp2.LocalID; - if (p2events) - cp2.AddCollisionEvent(0, contact); - } - break; + AddCollisionEventReporting(p2); + p2.AddCollisionEvent(0, contact); } break; - } + } } + } /// /// This is our collision testing routine in ODE @@ -2369,6 +2252,7 @@ namespace OpenSim.Region.Physics.OdePlugin RegionTerrain.Remove(pOffset); if (GroundGeom != IntPtr.Zero) { + actor_name_map.Remove(GroundGeom); d.GeomDestroy(GroundGeom); if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) @@ -2394,27 +2278,32 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); d.GeomSetCollideBits(GroundGeom, 0); - } - geom_name_map[GroundGeom] = "Terrain"; + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Terrain"; + pa.PhysicsActorType = (int)ActorTypes.Ground; + actor_name_map[GroundGeom] = pa; - d.Matrix3 R = new d.Matrix3(); +// geom_name_map[GroundGeom] = "Terrain"; - Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); - Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); - + d.Matrix3 R = new d.Matrix3(); - q1 = q1 * q2; - - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + + q1 = q1 * q2; + + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); - d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); - d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); - RegionTerrain.Add(pOffset, GroundGeom); - TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); - TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(GroundGeom, ref R); + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); + RegionTerrain.Add(pOffset, GroundGeom); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + } } } @@ -2478,6 +2367,7 @@ namespace OpenSim.Region.Physics.OdePlugin RegionTerrain.Remove(pOffset); if (GroundGeom != IntPtr.Zero) { + actor_name_map.Remove(GroundGeom); d.GeomDestroy(GroundGeom); if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) @@ -2509,13 +2399,18 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); d.GeomSetCollideBits(GroundGeom, 0); - } - geom_name_map[GroundGeom] = "Terrain"; + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Terrain"; + pa.PhysicsActorType = (int)ActorTypes.Ground; + actor_name_map[GroundGeom] = pa; - d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); - RegionTerrain.Add(pOffset, GroundGeom); - TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); - TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); +// geom_name_map[GroundGeom] = "Terrain"; + + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); + RegionTerrain.Add(pOffset, GroundGeom); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + } } } @@ -2632,6 +2527,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (WaterGeom != IntPtr.Zero) { + actor_name_map.Remove(WaterGeom); d.GeomDestroy(WaterGeom); d.GeomHeightfieldDataDestroy(WaterHeightmapData); WaterGeom = IntPtr.Zero; @@ -2654,7 +2550,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water)); d.GeomSetCollideBits(WaterGeom, 0); - geom_name_map[WaterGeom] = "Water"; + + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Water"; + pa.PhysicsActorType = (int)ActorTypes.Water; + + actor_name_map[WaterGeom] = pa; +// geom_name_map[WaterGeom] = "Water"; d.Matrix3 R = new d.Matrix3(); -- cgit v1.1 From 0d3661fb5f9f8f2cfc29d516d744ce87826ec6be Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 17 Jul 2012 00:27:01 +0100 Subject: UbitOde: remove useless water collider from active code. --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 139c4e8..a554897 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -203,9 +203,9 @@ namespace OpenSim.Region.Physics.OdePlugin private float waterlevel = 0f; private int framecount = 0; - private IntPtr WaterGeom = IntPtr.Zero; - private IntPtr WaterHeightmapData = IntPtr.Zero; - private GCHandle WaterMapHandler = new GCHandle(); +// private IntPtr WaterGeom = IntPtr.Zero; +// private IntPtr WaterHeightmapData = IntPtr.Zero; +// private GCHandle WaterMapHandler = new GCHandle(); public float avPIDD = 2200f; // make it visible public float avPIDP = 900f; // make it visible @@ -2491,9 +2491,9 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SetWaterLevel(float baseheight) { waterlevel = baseheight; - randomizeWater(waterlevel); +// randomizeWater(waterlevel); } - +/* public void randomizeWater(float baseheight) { const uint heightmapWidth = Constants.RegionSize + 2; @@ -2574,7 +2574,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } } - +*/ public override void Dispose() { lock (OdeLock) @@ -2624,7 +2624,7 @@ namespace OpenSim.Region.Physics.OdePlugin TerrainHeightFieldHeightsHandlers.Clear(); TerrainHeightFieldHeights.Clear(); - +/* if (WaterGeom != IntPtr.Zero) { d.GeomDestroy(WaterGeom); @@ -2636,7 +2636,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (WaterMapHandler.IsAllocated) WaterMapHandler.Free(); } - +*/ if (ContactgeomsArray != IntPtr.Zero) Marshal.FreeHGlobal(ContactgeomsArray); if (GlobalContactsArray != IntPtr.Zero) -- cgit v1.1 From 0facebec4076f682f79070e639905869161fd6b1 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 19 Jul 2012 11:12:53 +0100 Subject: Update ubitMesh primMesher with new Dahlia version on core ( reckick git back) --- OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs | 326 +++++++++++++---------- 1 file changed, 183 insertions(+), 143 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs index 53022ad..4049ee1 100644 --- a/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs +++ b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs @@ -236,6 +236,13 @@ namespace PrimMesher this.U = u; this.V = v; } + + public UVCoord Flip() + { + this.U = 1.0f - this.U; + this.V = 1.0f - this.V; + return this; + } } public struct Face @@ -603,40 +610,40 @@ namespace PrimMesher /// /// generates a profile for extrusion /// - internal class Profile + public class Profile { private const float twoPi = 2.0f * (float)Math.PI; - internal string errorMessage = null; + public string errorMessage = null; - internal List coords; - internal List faces; - internal List vertexNormals; - internal List us; - internal List faceUVs; - internal List faceNumbers; + public List coords; + public List faces; + public List vertexNormals; + public List us; + public List faceUVs; + public List faceNumbers; // use these for making individual meshes for each prim face - internal List outerCoordIndices = null; - internal List hollowCoordIndices = null; - internal List cut1CoordIndices = null; - internal List cut2CoordIndices = null; + public List outerCoordIndices = null; + public List hollowCoordIndices = null; + public List cut1CoordIndices = null; + public List cut2CoordIndices = null; - internal Coord faceNormal = new Coord(0.0f, 0.0f, 1.0f); - internal Coord cutNormal1 = new Coord(); - internal Coord cutNormal2 = new Coord(); + public Coord faceNormal = new Coord(0.0f, 0.0f, 1.0f); + public Coord cutNormal1 = new Coord(); + public Coord cutNormal2 = new Coord(); - internal int numOuterVerts = 0; - internal int numHollowVerts = 0; + public int numOuterVerts = 0; + public int numHollowVerts = 0; - internal int outerFaceNumber = -1; - internal int hollowFaceNumber = -1; + public int outerFaceNumber = -1; + public int hollowFaceNumber = -1; - internal bool calcVertexNormals = false; - internal int bottomFaceNumber = 0; - internal int numPrimFaces = 0; + public bool calcVertexNormals = false; + public int bottomFaceNumber = 0; + public int numPrimFaces = 0; - internal Profile() + public Profile() { this.coords = new List(); this.faces = new List(); @@ -646,7 +653,7 @@ namespace PrimMesher this.faceNumbers = new List(); } - internal Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool createFaces, bool calcVertexNormals) + public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool createFaces, bool calcVertexNormals) { this.calcVertexNormals = calcVertexNormals; this.coords = new List(); @@ -657,7 +664,6 @@ namespace PrimMesher this.faceNumbers = new List(); Coord center = new Coord(0.0f, 0.0f, 0.0f); - //bool hasCenter = false; List hollowCoords = new List(); List hollowNormals = new List(); @@ -682,8 +688,8 @@ namespace PrimMesher float yScale = 0.5f; if (sides == 4) // corners of a square are sqrt(2) from center { - xScale = 0.707f; - yScale = 0.707f; + xScale = 0.707107f; + yScale = 0.707107f; } float startAngle = profileStart * twoPi; @@ -724,7 +730,6 @@ namespace PrimMesher else if (!simpleFace) { this.coords.Add(center); - //hasCenter = true; if (this.calcVertexNormals) this.vertexNormals.Add(new Coord(0.0f, 0.0f, 1.0f)); this.us.Add(0.0f); @@ -752,7 +757,10 @@ namespace PrimMesher else hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f)); - hollowUs.Add(angle.angle * hollow); + if (hollowSides == 4) + hollowUs.Add(angle.angle * hollow * 0.707107f); + else + hollowUs.Add(angle.angle * hollow); } } } @@ -829,9 +837,6 @@ namespace PrimMesher if (createFaces) { - //int numOuterVerts = this.coords.Count; - //numOuterVerts = this.coords.Count; - //int numHollowVerts = hollowCoords.Count; int numTotalVerts = this.numOuterVerts + this.numHollowVerts; if (this.numOuterVerts == this.numHollowVerts) @@ -993,11 +998,7 @@ namespace PrimMesher if (startVert > 0) this.faceNumbers.Add(-1); for (int i = 0; i < this.numOuterVerts - 1; i++) - //this.faceNumbers.Add(sides < 5 ? faceNum++ : faceNum); - this.faceNumbers.Add(sides < 5 && i < sides ? faceNum++ : faceNum); - - //if (!hasHollow && !hasProfileCut) - // this.bottomFaceNumber = faceNum++; + this.faceNumbers.Add(sides < 5 && i <= sides ? faceNum++ : faceNum); this.faceNumbers.Add(hasProfileCut ? -1 : faceNum++); @@ -1014,8 +1015,7 @@ namespace PrimMesher this.hollowFaceNumber = faceNum++; } - //if (hasProfileCut || hasHollow) - // this.bottomFaceNumber = faceNum++; + this.bottomFaceNumber = faceNum++; if (hasHollow && hasProfileCut) @@ -1030,19 +1030,19 @@ namespace PrimMesher } - internal void MakeFaceUVs() + public void MakeFaceUVs() { this.faceUVs = new List(); foreach (Coord c in this.coords) - this.faceUVs.Add(new UVCoord(0.5f + c.X, 0.5f - c.Y)); + this.faceUVs.Add(new UVCoord(1.0f - (0.5f + c.X), 1.0f - (0.5f - c.Y))); } - internal Profile Copy() + public Profile Copy() { return this.Copy(true); } - internal Profile Copy(bool needFaces) + public Profile Copy(bool needFaces) { Profile copy = new Profile(); @@ -1071,12 +1071,12 @@ namespace PrimMesher return copy; } - internal void AddPos(Coord v) + public void AddPos(Coord v) { this.AddPos(v.X, v.Y, v.Z); } - internal void AddPos(float x, float y, float z) + public void AddPos(float x, float y, float z) { int i; int numVerts = this.coords.Count; @@ -1092,7 +1092,7 @@ namespace PrimMesher } } - internal void AddRot(Quat q) + public void AddRot(Quat q) { int i; int numVerts = this.coords.Count; @@ -1113,7 +1113,7 @@ namespace PrimMesher } } - internal void Scale(float x, float y) + public void Scale(float x, float y) { int i; int numVerts = this.coords.Count; @@ -1131,7 +1131,7 @@ namespace PrimMesher /// /// Changes order of the vertex indices and negates the center vertex normal. Does not alter vertex normals of radial vertices /// - internal void FlipNormals() + public void FlipNormals() { int i; int numFaces = this.faces.Count; @@ -1171,7 +1171,7 @@ namespace PrimMesher } } - internal void AddValue2FaceVertexIndices(int num) + public void AddValue2FaceVertexIndices(int num) { int numFaces = this.faces.Count; Face tmpFace; @@ -1186,7 +1186,7 @@ namespace PrimMesher } } - internal void AddValue2FaceNormalIndices(int num) + public void AddValue2FaceNormalIndices(int num) { if (this.calcVertexNormals) { @@ -1204,7 +1204,7 @@ namespace PrimMesher } } - internal void DumpRaw(String path, String name, String title) + public void DumpRaw(String path, String name, String title) { if (path == null) return; @@ -1261,6 +1261,15 @@ namespace PrimMesher public void Create(PathType pathType, int steps) { + if (this.taperX > 0.999f) + this.taperX = 0.999f; + if (this.taperX < -0.999f) + this.taperX = -0.999f; + if (this.taperY > 0.999f) + this.taperY = 0.999f; + if (this.taperY < -0.999f) + this.taperY = -0.999f; + if (pathType == PathType.Linear || pathType == PathType.Flexible) { int step = 0; @@ -1273,12 +1282,12 @@ namespace PrimMesher float start = -0.5f; float stepSize = length / (float)steps; - float percentOfPathMultiplier = stepSize; - float xOffset = 0.0f; - float yOffset = 0.0f; + float percentOfPathMultiplier = stepSize * 0.999999f; + float xOffset = this.topShearX * this.pathCutBegin; + float yOffset = this.topShearY * this.pathCutBegin; float zOffset = start; - float xOffsetStepIncrement = this.topShearX / steps; - float yOffsetStepIncrement = this.topShearY / steps; + float xOffsetStepIncrement = this.topShearX * length / steps; + float yOffsetStepIncrement = this.topShearY * length / steps; float percentOfPath = this.pathCutBegin; zOffset += percentOfPath; @@ -1573,13 +1582,6 @@ namespace PrimMesher this.hollow = 0.99f; if (hollow < 0.0f) this.hollow = 0.0f; - - //if (sphereMode) - // this.hasProfileCut = this.profileEnd - this.profileStart < 0.4999f; - //else - // //this.hasProfileCut = (this.profileStart > 0.0f || this.profileEnd < 1.0f); - // this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; - //this.hasHollow = (this.hollow > 0.001f); } /// @@ -1614,10 +1616,9 @@ namespace PrimMesher steps = (int)(steps * 4.5 * length); } - if (sphereMode) + if (this.sphereMode) this.hasProfileCut = this.profileEnd - this.profileStart < 0.4999f; else - //this.hasProfileCut = (this.profileStart > 0.0f || this.profileEnd < 1.0f); this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; this.hasHollow = (this.hollow > 0.001f); @@ -1630,6 +1631,22 @@ namespace PrimMesher float hollow = this.hollow; + if (pathType == PathType.Circular) + { + needEndFaces = false; + if (this.pathCutBegin != 0.0f || this.pathCutEnd != 1.0f) + needEndFaces = true; + else if (this.taperX != 0.0f || this.taperY != 0.0f) + needEndFaces = true; + else if (this.skew != 0.0f) + needEndFaces = true; + else if (twistTotal != 0.0f) + needEndFaces = true; + else if (this.radius != 0.0f) + needEndFaces = true; + } + else needEndFaces = true; + // sanity checks float initialProfileRot = 0.0f; if (pathType == PathType.Circular) @@ -1689,20 +1706,13 @@ namespace PrimMesher this.numPrimFaces = profile.numPrimFaces; - //profileOuterFaceNumber = profile.faceNumbers[0]; - //if (!needEndFaces) - // profileOuterFaceNumber--; - //profileOuterFaceNumber = needEndFaces ? 1 : 0; - - - //if (hasHollow) - //{ - // if (needEndFaces) - // profileHollowFaceNumber = profile.faceNumbers[profile.numOuterVerts + 1]; - // else - // profileHollowFaceNumber = profile.faceNumbers[profile.numOuterVerts] - 1; - //} - + int cut1FaceNumber = profile.bottomFaceNumber + 1; + int cut2FaceNumber = cut1FaceNumber + 1; + if (!needEndFaces) + { + cut1FaceNumber -= 2; + cut2FaceNumber -= 2; + } profileOuterFaceNumber = profile.outerFaceNumber; if (!needEndFaces) @@ -1732,7 +1742,8 @@ namespace PrimMesher Coord lastCutNormal1 = new Coord(); Coord lastCutNormal2 = new Coord(); - float lastV = 1.0f; + float thisV = 0.0f; + float lastV = 0.0f; Path path = new Path(); path.twistBegin = twistBegin; @@ -1754,23 +1765,6 @@ namespace PrimMesher path.Create(pathType, steps); - - if (pathType == PathType.Circular) - { - needEndFaces = false; - if (this.pathCutBegin != 0.0f || this.pathCutEnd != 1.0f) - needEndFaces = true; - else if (this.taperX != 0.0f || this.taperY != 0.0f) - needEndFaces = true; - else if (this.skew != 0.0f) - needEndFaces = true; - else if (twistTotal != 0.0f) - needEndFaces = true; - else if (this.radius != 0.0f) - needEndFaces = true; - } - else needEndFaces = true; - for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) { PathNode node = path.pathNodes[nodeIndex]; @@ -1784,7 +1778,7 @@ namespace PrimMesher { newLayer.FlipNormals(); - // add the top faces to the viewerFaces list here + // add the bottom faces to the viewerFaces list if (this.viewerMode) { Coord faceNormal = newLayer.faceNormal; @@ -1811,6 +1805,13 @@ namespace PrimMesher newViewerFace.uv2 = newLayer.faceUVs[face.v2]; newViewerFace.uv3 = newLayer.faceUVs[face.v3]; + if (pathType == PathType.Linear) + { + newViewerFace.uv1.Flip(); + newViewerFace.uv2.Flip(); + newViewerFace.uv3.Flip(); + } + this.viewerFaces.Add(newViewerFace); } } @@ -1835,7 +1836,10 @@ namespace PrimMesher // fill faces between layers int numVerts = newLayer.coords.Count; - Face newFace = new Face(); + Face newFace1 = new Face(); + Face newFace2 = new Face(); + + thisV = 1.0f - node.percentOfPath; if (nodeIndex > 0) { @@ -1853,14 +1857,23 @@ namespace PrimMesher int whichVert = i - startVert; - newFace.v1 = i; - newFace.v2 = i - numVerts; - newFace.v3 = iNext - numVerts; - this.faces.Add(newFace); + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = iNext; + + newFace1.n1 = newFace1.v1; + newFace1.n2 = newFace1.v2; + newFace1.n3 = newFace1.v3; + this.faces.Add(newFace1); - newFace.v2 = iNext - numVerts; - newFace.v3 = iNext; - this.faces.Add(newFace); + newFace2.v1 = iNext; + newFace2.v2 = i - numVerts; + newFace2.v3 = iNext - numVerts; + + newFace2.n1 = newFace2.v1; + newFace2.n2 = newFace2.v2; + newFace2.n3 = newFace2.v3; + this.faces.Add(newFace2); if (this.viewerMode) { @@ -1873,10 +1886,16 @@ namespace PrimMesher ViewerFace newViewerFace1 = new ViewerFace(primFaceNum); ViewerFace newViewerFace2 = new ViewerFace(primFaceNum); - float u1 = newLayer.us[whichVert]; + int uIndex = whichVert; + if (!hasHollow && sides > 4 && uIndex < newLayer.us.Count - 1) + { + uIndex++; + } + + float u1 = newLayer.us[uIndex]; float u2 = 1.0f; - if (whichVert < newLayer.us.Count - 1) - u2 = newLayer.us[whichVert + 1]; + if (uIndex < (int)newLayer.us.Count - 1) + u2 = newLayer.us[uIndex + 1]; if (whichVert == cut1Vert || whichVert == cut2Vert) { @@ -1894,13 +1913,22 @@ namespace PrimMesher u1 -= (int)u1; if (u2 < 0.1f) u2 = 1.0f; - //this.profileOuterFaceNumber = primFaceNum; } - else if (whichVert > profile.coords.Count - profile.numHollowVerts - 1) + } + + if (this.sphereMode) + { + if (whichVert != cut1Vert && whichVert != cut2Vert) { - u1 *= 2.0f; - u2 *= 2.0f; - //this.profileHollowFaceNumber = primFaceNum; + u1 = u1 * 2.0f - 1.0f; + u2 = u2 * 2.0f - 1.0f; + + if (whichVert >= newLayer.numOuterVerts) + { + u1 -= hollow; + u2 -= hollow; + } + } } @@ -1908,37 +1936,39 @@ namespace PrimMesher newViewerFace1.uv2.U = u1; newViewerFace1.uv3.U = u2; - newViewerFace1.uv1.V = 1.0f - node.percentOfPath; + newViewerFace1.uv1.V = thisV; newViewerFace1.uv2.V = lastV; - newViewerFace1.uv3.V = lastV; + newViewerFace1.uv3.V = thisV; - newViewerFace2.uv1.U = u1; - newViewerFace2.uv2.U = u2; + newViewerFace2.uv1.U = u2; + newViewerFace2.uv2.U = u1; newViewerFace2.uv3.U = u2; - newViewerFace2.uv1.V = 1.0f - node.percentOfPath; + newViewerFace2.uv1.V = thisV; newViewerFace2.uv2.V = lastV; - newViewerFace2.uv3.V = 1.0f - node.percentOfPath; + newViewerFace2.uv3.V = lastV; - newViewerFace1.v1 = this.coords[i]; - newViewerFace1.v2 = this.coords[i - numVerts]; - newViewerFace1.v3 = this.coords[iNext - numVerts]; + newViewerFace1.v1 = this.coords[newFace1.v1]; + newViewerFace1.v2 = this.coords[newFace1.v2]; + newViewerFace1.v3 = this.coords[newFace1.v3]; - newViewerFace2.v1 = this.coords[i]; - newViewerFace2.v2 = this.coords[iNext - numVerts]; - newViewerFace2.v3 = this.coords[iNext]; + newViewerFace2.v1 = this.coords[newFace2.v1]; + newViewerFace2.v2 = this.coords[newFace2.v2]; + newViewerFace2.v3 = this.coords[newFace2.v3]; - newViewerFace1.coordIndex1 = i; - newViewerFace1.coordIndex2 = i - numVerts; - newViewerFace1.coordIndex3 = iNext - numVerts; + newViewerFace1.coordIndex1 = newFace1.v1; + newViewerFace1.coordIndex2 = newFace1.v2; + newViewerFace1.coordIndex3 = newFace1.v3; - newViewerFace2.coordIndex1 = i; - newViewerFace2.coordIndex2 = iNext - numVerts; - newViewerFace2.coordIndex3 = iNext; + newViewerFace2.coordIndex1 = newFace2.v1; + newViewerFace2.coordIndex2 = newFace2.v2; + newViewerFace2.coordIndex3 = newFace2.v3; // profile cut faces if (whichVert == cut1Vert) { + newViewerFace1.primFaceNumber = cut1FaceNumber; + newViewerFace2.primFaceNumber = cut1FaceNumber; newViewerFace1.n1 = newLayer.cutNormal1; newViewerFace1.n2 = newViewerFace1.n3 = lastCutNormal1; @@ -1947,10 +1977,14 @@ namespace PrimMesher } else if (whichVert == cut2Vert) { + newViewerFace1.primFaceNumber = cut2FaceNumber; + newViewerFace2.primFaceNumber = cut2FaceNumber; newViewerFace1.n1 = newLayer.cutNormal2; - newViewerFace1.n2 = newViewerFace1.n3 = lastCutNormal2; + newViewerFace1.n2 = lastCutNormal2; + newViewerFace1.n3 = lastCutNormal2; - newViewerFace2.n1 = newViewerFace2.n3 = newLayer.cutNormal2; + newViewerFace2.n1 = newLayer.cutNormal2; + newViewerFace2.n3 = newLayer.cutNormal2; newViewerFace2.n2 = lastCutNormal2; } @@ -1963,13 +1997,13 @@ namespace PrimMesher } else { - newViewerFace1.n1 = this.normals[i]; - newViewerFace1.n2 = this.normals[i - numVerts]; - newViewerFace1.n3 = this.normals[iNext - numVerts]; + newViewerFace1.n1 = this.normals[newFace1.n1]; + newViewerFace1.n2 = this.normals[newFace1.n2]; + newViewerFace1.n3 = this.normals[newFace1.n3]; - newViewerFace2.n1 = this.normals[i]; - newViewerFace2.n2 = this.normals[iNext - numVerts]; - newViewerFace2.n3 = this.normals[iNext]; + newViewerFace2.n1 = this.normals[newFace2.n1]; + newViewerFace2.n2 = this.normals[newFace2.n2]; + newViewerFace2.n3 = this.normals[newFace2.n3]; } } @@ -1982,14 +2016,13 @@ namespace PrimMesher lastCutNormal1 = newLayer.cutNormal1; lastCutNormal2 = newLayer.cutNormal2; - lastV = 1.0f - node.percentOfPath; + lastV = thisV; if (needEndFaces && nodeIndex == path.pathNodes.Count - 1 && viewerMode) { // add the top faces to the viewerFaces list here Coord faceNormal = newLayer.faceNormal; - ViewerFace newViewerFace = new ViewerFace(); - newViewerFace.primFaceNumber = 0; + ViewerFace newViewerFace = new ViewerFace(0); int numFaces = newLayer.faces.Count; List faces = newLayer.faces; @@ -2012,6 +2045,13 @@ namespace PrimMesher newViewerFace.uv2 = newLayer.faceUVs[face.v2 - coordsLen]; newViewerFace.uv3 = newLayer.faceUVs[face.v3 - coordsLen]; + if (pathType == PathType.Linear) + { + newViewerFace.uv1.Flip(); + newViewerFace.uv2.Flip(); + newViewerFace.uv3.Flip(); + } + this.viewerFaces.Add(newViewerFace); } } -- cgit v1.1 From be22e3599cece284f981d0a98224dd563c3d933f Mon Sep 17 00:00:00 2001 From: Melanie Date: Fri, 27 Jul 2012 03:24:45 +0200 Subject: Change the stair fudge factor so steps of 0.5m can be climbled to match inworldz claims and SL's realities --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index a554897..f3ac3ca 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1027,7 +1027,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.AABB aabb; d.GeomGetAABB(g2, out aabb); - float tmp = vtmp.Z - sz * .25f; + float tmp = vtmp.Z - sz * .18f; if (aabb.MaxZ < tmp) { @@ -1057,7 +1057,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.AABB aabb; d.GeomGetAABB(g1, out aabb); - float tmp = vtmp.Z - sz * .25f; + float tmp = vtmp.Z - sz * .18f; if (aabb.MaxZ < tmp) { -- cgit v1.1 From 92d44446faad9fef1ec5008b84bf44ab9398f6a9 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 4 Aug 2012 17:33:14 +0100 Subject: *feature test* ubitode, let convex hull shape type work for prims other than uploaded meshs, making it change the mesh level of detail from high to low. This will work on all prims that get a internal mesh or sculpts. Mesh size reduction will depend on particular shape. This is not as SL. There prims do also get concave areas. Uploaded meshs work as before. A normal 10x10x10 torus gets 152 vertices in place of 900, and 198 faces in place of 1198. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 6bf5be1..a3534a4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1092,18 +1092,21 @@ namespace OpenSim.Region.Physics.OdePlugin CalcPrimBodyData(); m_mesh = null; - if (_parent_scene.needsMeshing(pbs)) + if (_parent_scene.needsMeshing(pbs) && (pbs.SculptData.Length > 0)) { bool convex; + int clod = (int)LevelOfDetail.High; if (m_shapetype == 0) convex = false; else + { convex = true; - - m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex); + if (_pbs.SculptType != (byte)SculptType.Mesh) + clod = (int)LevelOfDetail.Low; + } + m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, clod, true, convex); } - m_building = true; // control must set this to false when done AddChange(changes.Add, null); @@ -1360,12 +1363,18 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_mesh == null) { bool convex; + int clod = (int)LevelOfDetail.High; + if (m_shapetype == 0) convex = false; else + { convex = true; + if (_pbs.SculptType != (byte)SculptType.Mesh) + clod = (int)LevelOfDetail.Low; + } - mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex); + mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, clod, true, convex); } else { @@ -1373,7 +1382,7 @@ namespace OpenSim.Region.Physics.OdePlugin } if (mesh == null) - { + { m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); return false; } -- cgit v1.1 From 493309d91a2aaa56d95d3f08806524159bc6e645 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 5 Aug 2012 10:37:25 +0100 Subject: ubitmeshing: mask out mirror and invert bits on sculpttype convertion. Remove some unused --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 6 +- OpenSim/Region/Physics/UbitMeshing/SculptMap.cs | 14 +- OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs | 441 +--------------------- 3 files changed, 12 insertions(+), 449 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index f002bba..88efd6d 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -780,7 +780,9 @@ namespace OpenSim.Region.Physics.Meshing } PrimMesher.SculptMesh.SculptType sculptType; - switch ((OpenMetaverse.SculptType)primShape.SculptType) + // remove mirror and invert bits + OpenMetaverse.SculptType pbsSculptType = ((OpenMetaverse.SculptType)(primShape.SculptType & 0x3f)); + switch (pbsSculptType) { case OpenMetaverse.SculptType.Cylinder: sculptType = PrimMesher.SculptMesh.SculptType.cylinder; @@ -802,7 +804,7 @@ namespace OpenSim.Region.Physics.Meshing bool mirror = ((primShape.SculptType & 128) != 0); bool invert = ((primShape.SculptType & 64) != 0); - sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, false, mirror, invert); + sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, mirror, invert); idata.Dispose(); diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs index b3d9cb6..054caf3 100644 --- a/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs +++ b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs @@ -25,14 +25,10 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -// to build without references to System.Drawing, comment this out -#define SYSTEM_DRAWING - using System; using System.Collections.Generic; using System.Text; -#if SYSTEM_DRAWING using System.Drawing; using System.Drawing.Imaging; @@ -140,7 +136,6 @@ namespace PrimMesher int rowNdx, colNdx; int smNdx = 0; - for (rowNdx = 0; rowNdx < numRows; rowNdx++) { List row = new List(numCols); @@ -163,11 +158,11 @@ namespace PrimMesher { Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); - + Color c; float xscale = srcImage.Width / destWidth; float yscale = srcImage.Height / destHeight; - + float sy = 0.5f; for (int y = 0; y < destHeight; y++) { @@ -190,8 +185,5 @@ namespace PrimMesher srcImage.Dispose(); return scaledImage; } - - } - } -#endif +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs index 4a7f3ad..655b325 100644 --- a/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs @@ -25,18 +25,13 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -// to build without references to System.Drawing, comment this out -#define SYSTEM_DRAWING - using System; using System.Collections.Generic; using System.Text; using System.IO; -#if SYSTEM_DRAWING using System.Drawing; using System.Drawing.Imaging; -#endif namespace PrimMesher { @@ -46,274 +41,23 @@ namespace PrimMesher public List coords; public List faces; - public List viewerFaces; - public List normals; - public List uvs; - public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 }; -#if SYSTEM_DRAWING - - public SculptMesh SculptMeshFromFile(string fileName, SculptType sculptType, int lod, bool viewerMode) - { - Bitmap bitmap = (Bitmap)Bitmap.FromFile(fileName); - SculptMesh sculptMesh = new SculptMesh(bitmap, sculptType, lod, viewerMode); - bitmap.Dispose(); - return sculptMesh; - } - - - public SculptMesh(string fileName, int sculptType, int lod, int viewerMode, int mirror, int invert) - { - Bitmap bitmap = (Bitmap)Bitmap.FromFile(fileName); - _SculptMesh(bitmap, (SculptType)sculptType, lod, viewerMode != 0, mirror != 0, invert != 0); - bitmap.Dispose(); - } -#endif - - /// - /// ** Experimental ** May disappear from future versions ** not recommeneded for use in applications - /// Construct a sculpt mesh from a 2D array of floats - /// - /// - /// - /// - /// - /// - /// - public SculptMesh(float[,] zMap, float xBegin, float xEnd, float yBegin, float yEnd, bool viewerMode) - { - float xStep, yStep; - float uStep, vStep; - - int numYElements = zMap.GetLength(0); - int numXElements = zMap.GetLength(1); - - try - { - xStep = (xEnd - xBegin) / (float)(numXElements - 1); - yStep = (yEnd - yBegin) / (float)(numYElements - 1); - - uStep = 1.0f / (numXElements - 1); - vStep = 1.0f / (numYElements - 1); - } - catch (DivideByZeroException) - { - return; - } - - coords = new List(); - faces = new List(); - normals = new List(); - uvs = new List(); - - viewerFaces = new List(); - - int p1, p2, p3, p4; - - int x, y; - int xStart = 0, yStart = 0; - - for (y = yStart; y < numYElements; y++) - { - int rowOffset = y * numXElements; - - for (x = xStart; x < numXElements; x++) - { - /* - * p1-----p2 - * | \ f2 | - * | \ | - * | f1 \| - * p3-----p4 - */ - - p4 = rowOffset + x; - p3 = p4 - 1; - - p2 = p4 - numXElements; - p1 = p3 - numXElements; - - Coord c = new Coord(xBegin + x * xStep, yBegin + y * yStep, zMap[y, x]); - this.coords.Add(c); - if (viewerMode) - { - this.normals.Add(new Coord()); - this.uvs.Add(new UVCoord(uStep * x, 1.0f - vStep * y)); - } - - if (y > 0 && x > 0) - { - Face f1, f2; - - if (viewerMode) - { - f1 = new Face(p1, p4, p3, p1, p4, p3); - f1.uv1 = p1; - f1.uv2 = p4; - f1.uv3 = p3; - - f2 = new Face(p1, p2, p4, p1, p2, p4); - f2.uv1 = p1; - f2.uv2 = p2; - f2.uv3 = p4; - } - else - { - f1 = new Face(p1, p4, p3); - f2 = new Face(p1, p2, p4); - } - - this.faces.Add(f1); - this.faces.Add(f2); - } - } - } - - if (viewerMode) - calcVertexNormals(SculptType.plane, numXElements, numYElements); - } - -#if SYSTEM_DRAWING - public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode) - { - _SculptMesh(sculptBitmap, sculptType, lod, viewerMode, false, false); - } - - public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode, bool mirror, bool invert) - { - _SculptMesh(sculptBitmap, sculptType, lod, viewerMode, mirror, invert); - } -#endif - - public SculptMesh(List> rows, SculptType sculptType, bool viewerMode, bool mirror, bool invert) - { - _SculptMesh(rows, sculptType, viewerMode, mirror, invert); - } - -#if SYSTEM_DRAWING - /// - /// converts a bitmap to a list of lists of coords, while scaling the image. - /// the scaling is done in floating point so as to allow for reduced vertex position - /// quantization as the position will be averaged between pixel values. this routine will - /// likely fail if the bitmap width and height are not powers of 2. - /// - /// - /// - /// - /// - private List> bitmap2Coords(Bitmap bitmap, int scale, bool mirror) - { - int numRows = bitmap.Height / scale; - int numCols = bitmap.Width / scale; - List> rows = new List>(numRows); - - float pixScale = 1.0f / (scale * scale); - pixScale /= 255; - - int imageX, imageY = 0; - - int rowNdx, colNdx; - - for (rowNdx = 0; rowNdx < numRows; rowNdx++) - { - List row = new List(numCols); - for (colNdx = 0; colNdx < numCols; colNdx++) - { - imageX = colNdx * scale; - int imageYStart = rowNdx * scale; - int imageYEnd = imageYStart + scale; - int imageXEnd = imageX + scale; - float rSum = 0.0f; - float gSum = 0.0f; - float bSum = 0.0f; - for (; imageX < imageXEnd; imageX++) - { - for (imageY = imageYStart; imageY < imageYEnd; imageY++) - { - Color c = bitmap.GetPixel(imageX, imageY); - if (c.A != 255) - { - bitmap.SetPixel(imageX, imageY, Color.FromArgb(255, c.R, c.G, c.B)); - c = bitmap.GetPixel(imageX, imageY); - } - rSum += c.R; - gSum += c.G; - bSum += c.B; - } - } - if (mirror) - row.Add(new Coord(-(rSum * pixScale - 0.5f), gSum * pixScale - 0.5f, bSum * pixScale - 0.5f)); - else - row.Add(new Coord(rSum * pixScale - 0.5f, gSum * pixScale - 0.5f, bSum * pixScale - 0.5f)); - - } - rows.Add(row); - } - return rows; - } - private List> bitmap2CoordsSampled(Bitmap bitmap, int scale, bool mirror) + public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool mirror, bool invert) { - int numRows = bitmap.Height / scale; - int numCols = bitmap.Width / scale; - List> rows = new List>(numRows); - - float pixScale = 1.0f / 256.0f; - - int imageX, imageY = 0; - - int rowNdx, colNdx; - - for (rowNdx = 0; rowNdx <= numRows; rowNdx++) - { - List row = new List(numCols); - imageY = rowNdx * scale; - if (rowNdx == numRows) imageY--; - for (colNdx = 0; colNdx <= numCols; colNdx++) - { - imageX = colNdx * scale; - if (colNdx == numCols) imageX--; - - Color c = bitmap.GetPixel(imageX, imageY); - if (c.A != 255) - { - bitmap.SetPixel(imageX, imageY, Color.FromArgb(255, c.R, c.G, c.B)); - c = bitmap.GetPixel(imageX, imageY); - } - - if (mirror) - row.Add(new Coord(-(c.R * pixScale - 0.5f), c.G * pixScale - 0.5f, c.B * pixScale - 0.5f)); - else - row.Add(new Coord(c.R * pixScale - 0.5f, c.G * pixScale - 0.5f, c.B * pixScale - 0.5f)); - - } - rows.Add(row); - } - return rows; - } - - - void _SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode, bool mirror, bool invert) - { - _SculptMesh(new SculptMap(sculptBitmap, lod).ToRows(mirror), sculptType, viewerMode, mirror, invert); + if (mirror) + invert = !invert; + _SculptMesh(new SculptMap(sculptBitmap, lod).ToRows(mirror), sculptType, invert); } -#endif - void _SculptMesh(List> rows, SculptType sculptType, bool viewerMode, bool mirror, bool invert) + private void _SculptMesh(List> rows, SculptType sculptType, bool invert) { coords = new List(); faces = new List(); - normals = new List(); - uvs = new List(); sculptType = (SculptType)(((int)sculptType) & 0x07); - if (mirror) - invert = !invert; - - viewerFaces = new List(); - int width = rows[0].Count; int p1, p2, p3, p4; @@ -375,7 +119,6 @@ namespace PrimMesher int coordsDown = rows.Count; int coordsAcross = rows[0].Count; -// int lastColumn = coordsAcross - 1; float widthUnit = 1.0f / (coordsAcross - 1); float heightUnit = 1.0f / (coordsDown - 1); @@ -401,45 +144,11 @@ namespace PrimMesher p1 = p3 - coordsAcross; this.coords.Add(rows[imageY][imageX]); - if (viewerMode) - { - this.normals.Add(new Coord()); - this.uvs.Add(new UVCoord(widthUnit * imageX, heightUnit * imageY)); - } if (imageY > 0 && imageX > 0) { Face f1, f2; - if (viewerMode) - { - if (invert) - { - f1 = new Face(p1, p4, p3, p1, p4, p3); - f1.uv1 = p1; - f1.uv2 = p4; - f1.uv3 = p3; - - f2 = new Face(p1, p2, p4, p1, p2, p4); - f2.uv1 = p1; - f2.uv2 = p2; - f2.uv3 = p4; - } - else - { - f1 = new Face(p1, p3, p4, p1, p3, p4); - f1.uv1 = p1; - f1.uv2 = p3; - f1.uv3 = p4; - - f2 = new Face(p1, p4, p2, p1, p4, p2); - f2.uv1 = p1; - f2.uv2 = p4; - f2.uv3 = p2; - } - } - else - { if (invert) { f1 = new Face(p1, p4, p3); @@ -450,16 +159,12 @@ namespace PrimMesher f1 = new Face(p1, p3, p4); f2 = new Face(p1, p4, p2); } - } this.faces.Add(f1); this.faces.Add(f2); } } } - - if (viewerMode) - calcVertexNormals(sculptType, coordsAcross, coordsDown); } /// @@ -475,129 +180,6 @@ namespace PrimMesher { coords = new List(sm.coords); faces = new List(sm.faces); - viewerFaces = new List(sm.viewerFaces); - normals = new List(sm.normals); - uvs = new List(sm.uvs); - } - - private void calcVertexNormals(SculptType sculptType, int xSize, int ySize) - { // compute vertex normals by summing all the surface normals of all the triangles sharing - // each vertex and then normalizing - int numFaces = this.faces.Count; - for (int i = 0; i < numFaces; i++) - { - Face face = this.faces[i]; - Coord surfaceNormal = face.SurfaceNormal(this.coords); - this.normals[face.n1] += surfaceNormal; - this.normals[face.n2] += surfaceNormal; - this.normals[face.n3] += surfaceNormal; - } - - int numNormals = this.normals.Count; - for (int i = 0; i < numNormals; i++) - this.normals[i] = this.normals[i].Normalize(); - - if (sculptType != SculptType.plane) - { // blend the vertex normals at the cylinder seam - for (int y = 0; y < ySize; y++) - { - int rowOffset = y * xSize; - - this.normals[rowOffset] = this.normals[rowOffset + xSize - 1] = (this.normals[rowOffset] + this.normals[rowOffset + xSize - 1]).Normalize(); - } - } - - foreach (Face face in this.faces) - { - ViewerFace vf = new ViewerFace(0); - vf.v1 = this.coords[face.v1]; - vf.v2 = this.coords[face.v2]; - vf.v3 = this.coords[face.v3]; - - vf.coordIndex1 = face.v1; - vf.coordIndex2 = face.v2; - vf.coordIndex3 = face.v3; - - vf.n1 = this.normals[face.n1]; - vf.n2 = this.normals[face.n2]; - vf.n3 = this.normals[face.n3]; - - vf.uv1 = this.uvs[face.uv1]; - vf.uv2 = this.uvs[face.uv2]; - vf.uv3 = this.uvs[face.uv3]; - - this.viewerFaces.Add(vf); - } - } - - /// - /// Adds a value to each XYZ vertex coordinate in the mesh - /// - /// - /// - /// - public void AddPos(float x, float y, float z) - { - int i; - int numVerts = this.coords.Count; - Coord vert; - - for (i = 0; i < numVerts; i++) - { - vert = this.coords[i]; - vert.X += x; - vert.Y += y; - vert.Z += z; - this.coords[i] = vert; - } - - if (this.viewerFaces != null) - { - int numViewerFaces = this.viewerFaces.Count; - - for (i = 0; i < numViewerFaces; i++) - { - ViewerFace v = this.viewerFaces[i]; - v.AddPos(x, y, z); - this.viewerFaces[i] = v; - } - } - } - - /// - /// Rotates the mesh - /// - /// - public void AddRot(Quat q) - { - int i; - int numVerts = this.coords.Count; - - for (i = 0; i < numVerts; i++) - this.coords[i] *= q; - - int numNormals = this.normals.Count; - for (i = 0; i < numNormals; i++) - this.normals[i] *= q; - - if (this.viewerFaces != null) - { - int numViewerFaces = this.viewerFaces.Count; - - for (i = 0; i < numViewerFaces; i++) - { - ViewerFace v = this.viewerFaces[i]; - v.v1 *= q; - v.v2 *= q; - v.v3 *= q; - - v.n1 *= q; - v.n2 *= q; - v.n3 *= q; - - this.viewerFaces[i] = v; - } - } } public void Scale(float x, float y, float z) @@ -608,19 +190,6 @@ namespace PrimMesher Coord m = new Coord(x, y, z); for (i = 0; i < numVerts; i++) this.coords[i] *= m; - - if (this.viewerFaces != null) - { - int numViewerFaces = this.viewerFaces.Count; - for (i = 0; i < numViewerFaces; i++) - { - ViewerFace v = this.viewerFaces[i]; - v.v1 *= m; - v.v2 *= m; - v.v3 *= m; - this.viewerFaces[i] = v; - } - } } public void DumpRaw(String path, String name, String title) -- cgit v1.1 From 307c45af2abc3246ae93e4f068d1da7679957be3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 5 Aug 2012 12:54:34 +0100 Subject: bug fix: keep sculpt bitmaps border pixels during resolution scaling. let this eventually have diferent interpolator last steps on each direction as sl seems to do. --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 3 + OpenSim/Region/Physics/UbitMeshing/SculptMap.cs | 83 +++++++++++++++++++---- OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs | 7 +- 3 files changed, 78 insertions(+), 15 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 88efd6d..3b1bdfb 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -314,6 +314,9 @@ namespace OpenSim.Region.Physics.Meshing coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z)); } + coords.Clear(); + faces.Clear(); + return mesh; } diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs index 054caf3..1c75db6 100644 --- a/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs +++ b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs @@ -56,11 +56,12 @@ namespace PrimMesher int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image - bool smallMap = bmW * bmH <= numLodPixels; bool needsScaling = false; + bool smallMap = false; width = bmW; height = bmH; + while (width * height > numLodPixels * 4) { width >>= 1; @@ -81,9 +82,12 @@ namespace PrimMesher if (width * height > numLodPixels) { + smallMap = false; width >>= 1; height >>= 1; } + else + smallMap = true; int numBytes = (width + 1) * (height + 1); redBytes = new byte[numBytes]; @@ -91,21 +95,18 @@ namespace PrimMesher blueBytes = new byte[numBytes]; int byteNdx = 0; + Color c; try { for (int y = 0; y <= height; y++) { - for (int x = 0; x <= width; x++) + for (int x = 0; x < width; x++) { - Color c; - if (smallMap) - c = bm.GetPixel(x < width ? x : x - 1, - y < height ? y : y - 1); + c = bm.GetPixel(x, y < height ? y : y - 1); else - c = bm.GetPixel(x < width ? x * 2 : x * 2 - 1, - y < height ? y * 2 : y * 2 - 1); + c = bm.GetPixel(x * 2, y < height ? y * 2 : y * 2 - 1); redBytes[byteNdx] = c.R; greenBytes[byteNdx] = c.G; @@ -113,6 +114,17 @@ namespace PrimMesher ++byteNdx; } + + if (smallMap) + c = bm.GetPixel(width - 1, y < height ? y : y - 1); + else + c = bm.GetPixel(width * 2 - 1, y < height ? y * 2 : y * 2 - 1); + + redBytes[byteNdx] = c.R; + greenBytes[byteNdx] = c.G; + blueBytes[byteNdx] = c.B; + + ++byteNdx; } } catch (Exception e) @@ -160,14 +172,25 @@ namespace PrimMesher Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); Color c; - float xscale = srcImage.Width / destWidth; - float yscale = srcImage.Height / destHeight; + + + // will let last step to be eventually diferent, as seems to be in sl + + float xscale = (float)srcImage.Width / (float)destWidth; + float yscale = (float)srcImage.Height / (float)destHeight; + + int lastsx = srcImage.Width - 1; + int lastsy = srcImage.Height - 1; + int lastdx = destWidth - 1; + int lastdy = destHeight - 1; float sy = 0.5f; - for (int y = 0; y < destHeight; y++) + float sx; + + for (int y = 0; y < lastdy; y++) { - float sx = 0.5f; - for (int x = 0; x < destWidth; x++) + sx = 0.5f; + for (int x = 0; x < lastdx; x++) { try { @@ -177,11 +200,43 @@ namespace PrimMesher catch (IndexOutOfRangeException) { } - sx += xscale; } + try + { + c = srcImage.GetPixel(lastsx, (int)(sy)); + scaledImage.SetPixel(lastdx, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + sy += yscale; } + + sx = 0.5f; + for (int x = 0; x < lastdx; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), lastsy); + scaledImage.SetPixel(x, lastdy, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + + sx += xscale; + } + try + { + c = srcImage.GetPixel(lastsx, lastsy); + scaledImage.SetPixel(lastdx, lastdy, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + srcImage.Dispose(); return scaledImage; } diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs index 655b325..bc1375b 100644 --- a/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs @@ -48,7 +48,12 @@ namespace PrimMesher { if (mirror) invert = !invert; - _SculptMesh(new SculptMap(sculptBitmap, lod).ToRows(mirror), sculptType, invert); + + SculptMap smap = new SculptMap(sculptBitmap, lod); + + List> rows = smap.ToRows(mirror); + + _SculptMesh(rows, sculptType, invert); } private void _SculptMesh(List> rows, SculptType sculptType, bool invert) -- cgit v1.1 From 36a1248b317cd80717fef6bc7c8fab318172a075 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 6 Aug 2012 09:06:46 +0100 Subject: ** DANGER someone should stress test more ** release unused physics meshs, including unmanaged memory allocations (allocated by managed code) --- OpenSim/Region/Physics/Manager/IMesher.cs | 2 + OpenSim/Region/Physics/Manager/ZeroMesher.cs | 2 + OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 2 + OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 55 +++++----- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 117 ++++++++++++++++++++-- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 32 ++---- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 19 +++- 7 files changed, 174 insertions(+), 55 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index c32cf38..19aa747 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -37,6 +37,8 @@ namespace OpenSim.Region.Physics.Manager IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod); IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical); IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical,bool convex); + void ReleaseMesh(IMesh mesh); + void ExpireReleaseMeshs(); } // Values for level of detail to be passed to the mesher. diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs index 8a3b50b..f555cb9 100644 --- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs +++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs @@ -79,5 +79,7 @@ namespace OpenSim.Region.Physics.Manager return null; } + public void ReleaseMesh(IMesh mesh) { } + public void ExpireReleaseMeshs() { } } } diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index 825b858..3c4f737 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -763,5 +763,7 @@ namespace OpenSim.Region.Physics.Meshing return mesh; } + public void ReleaseMesh(IMesh imesh) { } + public void ExpireReleaseMeshs() { } } } diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index c715642..0727802 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -46,8 +46,8 @@ namespace OpenSim.Region.Physics.Meshing IntPtr m_indicesPtr = IntPtr.Zero; int m_indexCount = 0; public float[] m_normals; - Vector3 _centroid; - int _centroidDiv; + Vector3 m_centroid; + int m_centroidDiv; private class vertexcomp : IEqualityComparer { @@ -65,7 +65,6 @@ namespace OpenSim.Region.Physics.Meshing int c = v.Z.GetHashCode(); return (a << 16) ^ (b << 8) ^ c; } - } public Mesh() @@ -74,8 +73,18 @@ namespace OpenSim.Region.Physics.Meshing m_vertices = new Dictionary(vcomp); m_triangles = new List(); - _centroid = Vector3.Zero; - _centroidDiv = 0; + m_centroid = Vector3.Zero; + m_centroidDiv = 0; + } + + public int RefCount { get; set; } + + public ulong Key { get; set; } + + public void Scale(Vector3 scale) + { + + } public Mesh Clone() @@ -86,8 +95,8 @@ namespace OpenSim.Region.Physics.Meshing { result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); } - result._centroid = _centroid; - result._centroidDiv = _centroidDiv; + result.m_centroid = m_centroid; + result.m_centroidDiv = m_centroidDiv; return result; } @@ -109,41 +118,41 @@ namespace OpenSim.Region.Physics.Meshing if (m_vertices.Count == 0) { - _centroidDiv = 0; - _centroid = Vector3.Zero; + m_centroidDiv = 0; + m_centroid = Vector3.Zero; } if (!m_vertices.ContainsKey(triangle.v1)) { m_vertices[triangle.v1] = m_vertices.Count; - _centroid.X += triangle.v1.X; - _centroid.Y += triangle.v1.Y; - _centroid.Z += triangle.v1.Z; - _centroidDiv++; + m_centroid.X += triangle.v1.X; + m_centroid.Y += triangle.v1.Y; + m_centroid.Z += triangle.v1.Z; + m_centroidDiv++; } if (!m_vertices.ContainsKey(triangle.v2)) { m_vertices[triangle.v2] = m_vertices.Count; - _centroid.X += triangle.v2.X; - _centroid.Y += triangle.v2.Y; - _centroid.Z += triangle.v2.Z; - _centroidDiv++; + m_centroid.X += triangle.v2.X; + m_centroid.Y += triangle.v2.Y; + m_centroid.Z += triangle.v2.Z; + m_centroidDiv++; } if (!m_vertices.ContainsKey(triangle.v3)) { m_vertices[triangle.v3] = m_vertices.Count; - _centroid.X += triangle.v3.X; - _centroid.Y += triangle.v3.Y; - _centroid.Z += triangle.v3.Z; - _centroidDiv++; + m_centroid.X += triangle.v3.X; + m_centroid.Y += triangle.v3.Y; + m_centroid.Z += triangle.v3.Z; + m_centroidDiv++; } m_triangles.Add(triangle); } public Vector3 GetCentroid() { - if (_centroidDiv > 0) - return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv); + if (m_centroidDiv > 0) + return new Vector3(m_centroid.X / m_centroidDiv, m_centroid.Y / m_centroidDiv, m_centroid.Z / m_centroidDiv); else return Vector3.Zero; } diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 3b1bdfb..a894e5f 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -83,6 +83,7 @@ namespace OpenSim.Region.Physics.Meshing private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh private Dictionary m_uniqueMeshes = new Dictionary(); + private Dictionary m_uniqueReleasedMeshes = new Dictionary(); public Meshmerizer(IConfigSource config) { @@ -986,6 +987,8 @@ namespace OpenSim.Region.Physics.Meshing return CreateMesh(primName, primShape, size, lod, false,false); } + private static Vector3 m_MeshUnitSize = new Vector3(0.5f, 0.5f, 0.5f); + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) { #if SPAM @@ -995,17 +998,60 @@ namespace OpenSim.Region.Physics.Meshing Mesh mesh = null; ulong key = 0; - // If this mesh has been created already, return it instead of creating another copy - // For large regions with 100k+ prims and hundreds of copies of each, this can save a GB or more of memory - key = primShape.GetMeshKey(size, lod, convex); - if (m_uniqueMeshes.TryGetValue(key, out mesh)) - return mesh; - if (size.X < 0.01f) size.X = 0.01f; if (size.Y < 0.01f) size.Y = 0.01f; if (size.Z < 0.01f) size.Z = 0.01f; + // try to find a identical mesh on meshs in use + key = primShape.GetMeshKey(size, lod, convex); + + lock (m_uniqueMeshes) + { + m_uniqueMeshes.TryGetValue(key, out mesh); + + if (mesh != null) + { + mesh.RefCount++; + return mesh; + } + } + + // try to find a identical mesh on meshs recently released + lock (m_uniqueReleasedMeshes) + { + m_uniqueReleasedMeshes.TryGetValue(key, out mesh); + if (mesh != null) + { + m_uniqueReleasedMeshes.Remove(key); + lock (m_uniqueMeshes) + m_uniqueMeshes.Add(key, mesh); + mesh.RefCount = 1; + return mesh; + } + } +/* + Mesh UnitSizeMesh = null; + ulong unitsizekey = 0; + + unitsizekey = primShape.GetMeshKey(m_MeshUnitSize, lod, convex); + + lock(m_uniqueMeshes) + m_uniqueMeshes.TryGetValue(unitsizekey, out UnitSizeMesh); + + if (UnitSizeMesh !=null) + { + UnitSizeMesh.RefCount++; + mesh = UnitSizeMesh.Clone(); + mesh.Key = key; + mesh.Scale(size); + mesh.RefCount++; + lock(m_uniqueMeshes) + m_uniqueMeshes.Add(key, mesh); + return mesh; + } +*/ mesh = CreateMeshFromPrimMesher(primName, primShape, size, lod,convex); +// mesh.Key = unitsizekey; if (mesh != null) { @@ -1021,11 +1067,68 @@ namespace OpenSim.Region.Physics.Meshing // trim the vertex and triangle lists to free up memory mesh.TrimExcess(); + mesh.Key = key; + mesh.RefCount++; - m_uniqueMeshes.Add(key, mesh); + lock(m_uniqueMeshes) + m_uniqueMeshes.Add(key, mesh); } return mesh; } + + public void ReleaseMesh(IMesh imesh) + { + if (imesh == null) + return; + + Mesh mesh = (Mesh)imesh; + + int curRefCount = mesh.RefCount; + curRefCount--; + + if (curRefCount > 0) + { + mesh.RefCount = curRefCount; + return; + } + + lock (m_uniqueMeshes) + { + mesh.RefCount = 0; + m_uniqueMeshes.Remove(mesh.Key); + lock (m_uniqueReleasedMeshes) + m_uniqueReleasedMeshes.Add(mesh.Key, mesh); + } + } + + public void ExpireReleaseMeshs() + { + if (m_uniqueMeshes.Count == 0) + return; + + List meshstodelete = new List(); + int refcntr; + + lock (m_uniqueReleasedMeshes) + { + foreach (Mesh m in m_uniqueReleasedMeshes.Values) + { + refcntr = m.RefCount; + refcntr--; + if (refcntr > -6) + m.RefCount = refcntr; + else + meshstodelete.Add(m); + } + + foreach (Mesh m in meshstodelete) + { + m_uniqueReleasedMeshes.Remove(m.Key); + m.releaseSourceMeshData(); + m.releasePinned(); + } + } + } } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index a3534a4..fbc6134 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -57,7 +57,6 @@ using OdeAPI; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; - namespace OpenSim.Region.Physics.OdePlugin { public class OdePrim : PhysicsActor @@ -538,24 +537,6 @@ namespace OpenSim.Region.Physics.OdePlugin { set { -/* - IMesh mesh = null; - if (_parent_scene.needsMeshing(value)) - { - bool convex; - if (m_shapetype == 0) - convex = false; - else - convex = true; - mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex); - } - - if (mesh != null) - { - lock (m_meshlock) - m_mesh = mesh; - } -*/ AddChange(changes.Shape, value); } } @@ -1357,7 +1338,6 @@ namespace OpenSim.Region.Physics.OdePlugin IMesh mesh = null; - lock (m_meshlock) { if (m_mesh == null) @@ -1403,7 +1383,7 @@ namespace OpenSim.Region.Physics.OdePlugin hasOOBoffsetFromMesh = true; mesh.releaseSourceMeshData(); - m_mesh = null; + m_mesh = mesh; } IntPtr geo = IntPtr.Zero; @@ -1545,7 +1525,10 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomTriMeshDataDestroy(_triMeshData); _triMeshData = IntPtr.Zero; } + } + + // catch (System.AccessViolationException) catch (Exception e) { @@ -1559,6 +1542,13 @@ namespace OpenSim.Region.Physics.OdePlugin { m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); } + + if (m_mesh != null) + { + _parent_scene.mesher.ReleaseMesh(m_mesh); + m_mesh = null; + } + Body = IntPtr.Zero; hasOOBoffsetFromMesh = false; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index f3ac3ca..3ee5198 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -194,7 +194,8 @@ namespace OpenSim.Region.Physics.OdePlugin private float metersInSpace = 25.6f; private float m_timeDilation = 1.0f; - DateTime m_lastframe; + private DateTime m_lastframe; + private DateTime m_lastMeshExpire; public float gravityx = 0f; public float gravityy = 0f; @@ -203,6 +204,8 @@ namespace OpenSim.Region.Physics.OdePlugin private float waterlevel = 0f; private int framecount = 0; + private int m_meshExpireCntr; + // private IntPtr WaterGeom = IntPtr.Zero; // private IntPtr WaterHeightmapData = IntPtr.Zero; // private GCHandle WaterMapHandler = new GCHandle(); @@ -263,7 +266,6 @@ namespace OpenSim.Region.Physics.OdePlugin const int maxContactsbeforedeath = 4000; private volatile int m_global_contactcount = 0; - private IntPtr contactgroup; public ContactData[] m_materialContactsData = new ContactData[8]; @@ -594,6 +596,7 @@ namespace OpenSim.Region.Physics.OdePlugin } m_lastframe = DateTime.UtcNow; + m_lastMeshExpire = m_lastframe; } internal void waitForSpaceUnlock(IntPtr space) @@ -1768,9 +1771,9 @@ namespace OpenSim.Region.Physics.OdePlugin { DateTime now = DateTime.UtcNow; - TimeSpan SinceLastFrame = now - m_lastframe; + TimeSpan timedif = now - m_lastframe; m_lastframe = now; - timeStep = (float)SinceLastFrame.TotalSeconds; + timeStep = (float)timedif.TotalSeconds; // acumulate time so we can reduce error step_time += timeStep; @@ -1972,6 +1975,14 @@ namespace OpenSim.Region.Physics.OdePlugin _badCharacter.Clear(); } } + + timedif = now - m_lastMeshExpire; + + if (timedif.Seconds > 10) + { + mesher.ExpireReleaseMeshs(); + m_lastMeshExpire = now; + } /* int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); -- cgit v1.1 From eef6bb97c0f96a98468c44bae5392c9d129bfb1f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 7 Aug 2012 17:42:29 +0100 Subject: use a bit more complex mesh key identifier, plus a bug fix --- OpenSim/Region/Physics/Manager/IMesher.cs | 12 +++ OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 6 +- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 107 ++++++++++++++++------ 3 files changed, 96 insertions(+), 29 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index 19aa747..a8c99f7 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -27,6 +27,7 @@ using System; using System.Collections.Generic; +using System.Runtime.InteropServices; using OpenSim.Framework; using OpenMetaverse; @@ -56,6 +57,17 @@ namespace OpenSim.Region.Physics.Manager { } + [StructLayout(LayoutKind.Explicit)] + public struct AMeshKey + { + [FieldOffset(0)] + public UUID uuid; + [FieldOffset(0)] + public ulong hashA; + [FieldOffset(8)] + public ulong hashB; + } + public interface IMesh { List getVertexList(); diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index 0727802..98c0f0b 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -37,6 +37,7 @@ namespace OpenSim.Region.Physics.Meshing { public class Mesh : IMesh { + private Dictionary m_vertices; private List m_triangles; GCHandle m_pinnedVertexes; @@ -47,7 +48,8 @@ namespace OpenSim.Region.Physics.Meshing int m_indexCount = 0; public float[] m_normals; Vector3 m_centroid; - int m_centroidDiv; + int m_centroidDiv; + private class vertexcomp : IEqualityComparer { @@ -79,7 +81,7 @@ namespace OpenSim.Region.Physics.Meshing public int RefCount { get; set; } - public ulong Key { get; set; } + public AMeshKey Key { get; set; } public void Scale(Vector3 scale) { diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index a894e5f..4c40175 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -82,8 +82,10 @@ namespace OpenSim.Region.Physics.Meshing private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh - private Dictionary m_uniqueMeshes = new Dictionary(); - private Dictionary m_uniqueReleasedMeshes = new Dictionary(); +// private Dictionary m_uniqueMeshes = new Dictionary(); +// private Dictionary m_uniqueReleasedMeshes = new Dictionary(); + private Dictionary m_uniqueMeshes = new Dictionary(); + private Dictionary m_uniqueReleasedMeshes = new Dictionary(); public Meshmerizer(IConfigSource config) { @@ -977,6 +979,76 @@ namespace OpenSim.Region.Physics.Meshing return true; } + public AMeshKey GetMeshUniqueKey(PrimitiveBaseShape primShape, Vector3 size, byte lod, bool convex) + { + AMeshKey key = new AMeshKey(); + Byte[] someBytes; + + key.hashB = 5181; + ulong hash = 5381; + + if (primShape.SculptEntry) + { + key.uuid = primShape.SculptTexture; + key.hashB = mdjb2(key.hashB, primShape.SculptType); + } + else + { + hash = mdjb2(hash, primShape.PathCurve); + hash = mdjb2(hash, (byte)primShape.HollowShape); + hash = mdjb2(hash, (byte)primShape.ProfileShape); + hash = mdjb2(hash, primShape.PathBegin); + hash = mdjb2(hash, primShape.PathEnd); + hash = mdjb2(hash, primShape.PathScaleX); + hash = mdjb2(hash, primShape.PathScaleY); + hash = mdjb2(hash, primShape.PathShearX); + hash = mdjb2(hash, primShape.PathShearY); + hash = mdjb2(hash, (byte)primShape.PathTwist); + hash = mdjb2(hash, (byte)primShape.PathTwistBegin); + hash = mdjb2(hash, (byte)primShape.PathRadiusOffset); + hash = mdjb2(hash, (byte)primShape.PathTaperX); + hash = mdjb2(hash, (byte)primShape.PathTaperY); + hash = mdjb2(hash, primShape.PathRevolutions); + hash = mdjb2(hash, (byte)primShape.PathSkew); + hash = mdjb2(hash, primShape.ProfileBegin); + hash = mdjb2(hash, primShape.ProfileEnd); + hash = mdjb2(hash, primShape.ProfileHollow); + key.hashA = hash; + } + + hash = key.hashB; + + someBytes = size.GetBytes(); + for (int i = 0; i < someBytes.Length; i++) + hash = mdjb2(hash, someBytes[i]); + + hash = mdjb2(hash, lod); + + hash &= 0x3fffffffffffffff; + + if (convex) + hash |= 0x4000000000000000; + + if (primShape.SculptEntry) + hash |= 0x8000000000000000; + + key.hashB = hash; + + return key; + } + + private ulong mdjb2(ulong hash, byte c) + { + return ((hash << 5) + hash) + (ulong)c; + } + + private ulong mdjb2(ulong hash, ushort c) + { + hash = ((hash << 5) + hash) + (ulong)((byte)c); + return ((hash << 5) + hash) + (ulong)(c >> 8); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) { return CreateMesh(primName, primShape, size, lod, false,false); @@ -996,14 +1068,16 @@ namespace OpenSim.Region.Physics.Meshing #endif Mesh mesh = null; - ulong key = 0; +// ulong key = 0; + if (size.X < 0.01f) size.X = 0.01f; if (size.Y < 0.01f) size.Y = 0.01f; if (size.Z < 0.01f) size.Z = 0.01f; // try to find a identical mesh on meshs in use - key = primShape.GetMeshKey(size, lod, convex); +// key = primShape.GetMeshKey(size, lod, convex); + AMeshKey key = GetMeshUniqueKey(primShape,size,(byte)lod, convex); lock (m_uniqueMeshes) { @@ -1029,29 +1103,8 @@ namespace OpenSim.Region.Physics.Meshing return mesh; } } -/* - Mesh UnitSizeMesh = null; - ulong unitsizekey = 0; - - unitsizekey = primShape.GetMeshKey(m_MeshUnitSize, lod, convex); - lock(m_uniqueMeshes) - m_uniqueMeshes.TryGetValue(unitsizekey, out UnitSizeMesh); - - if (UnitSizeMesh !=null) - { - UnitSizeMesh.RefCount++; - mesh = UnitSizeMesh.Clone(); - mesh.Key = key; - mesh.Scale(size); - mesh.RefCount++; - lock(m_uniqueMeshes) - m_uniqueMeshes.Add(key, mesh); - return mesh; - } -*/ mesh = CreateMeshFromPrimMesher(primName, primShape, size, lod,convex); -// mesh.Key = unitsizekey; if (mesh != null) { @@ -1068,7 +1121,7 @@ namespace OpenSim.Region.Physics.Meshing // trim the vertex and triangle lists to free up memory mesh.TrimExcess(); mesh.Key = key; - mesh.RefCount++; + mesh.RefCount = 1; lock(m_uniqueMeshes) m_uniqueMeshes.Add(key, mesh); @@ -1104,7 +1157,7 @@ namespace OpenSim.Region.Physics.Meshing public void ExpireReleaseMeshs() { - if (m_uniqueMeshes.Count == 0) + if (m_uniqueReleasedMeshes.Count == 0) return; List meshstodelete = new List(); -- cgit v1.1 From f2308c819e3b8550507146f0490f2fc143a4abc1 Mon Sep 17 00:00:00 2001 From: Melanie Date: Mon, 17 Sep 2012 12:58:59 +0200 Subject: Remove spammy "axislock" debug. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index fbc6134..61d5a1d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -902,7 +902,7 @@ namespace OpenSim.Region.Physics.OdePlugin axis.X = (axis.X > 0) ? 1f : 0f; axis.Y = (axis.Y > 0) ? 1f : 0f; axis.Z = (axis.Z > 0) ? 1f : 0f; - m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); +// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); AddChange(changes.AngLock, axis); } else -- cgit v1.1 From 58e55ae0756250f58f56cb59f75d18a512f17815 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 27 Sep 2012 23:14:42 +0100 Subject: make some forgotten changes be done by simulation thread not by caller --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 149 +++++++++++++++++------ OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 11 ++ 2 files changed, 126 insertions(+), 34 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 61d5a1d..f2f4725 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -161,9 +161,6 @@ namespace OpenSim.Region.Physics.OdePlugin private List childrenPrim = new List(); - -// private bool m_throttleUpdates; -// private int throttleCounter; public float m_collisionscore; private int m_colliderfilter = 0; @@ -175,7 +172,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_lastUpdateSent; public IntPtr Body = IntPtr.Zero; -// public String Name { get; private set; } + private Vector3 _target_velocity; public Vector3 primOOBsize; // prim real dimensions from mesh @@ -366,12 +363,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool ThrottleUpdates {get;set;} -/* - { - get { return m_throttleUpdates; } - set { m_throttleUpdates = value; } - } -*/ + public override bool Stopped { get { return _zeroFlag; } @@ -418,7 +410,6 @@ namespace OpenSim.Region.Physics.OdePlugin public override Vector3 Force { - //get { return Vector3.Zero; } get { return m_force; } set { @@ -673,7 +664,7 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_buoyancy; } set { - m_buoyancy = value; + AddChange(changes.Buoyancy,value); } } @@ -691,28 +682,35 @@ namespace OpenSim.Region.Physics.OdePlugin { if (value.IsFinite()) { - m_PIDTarget = value; + AddChange(changes.PIDTarget,value); } else m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); } } - public override bool PIDActive { set { m_usePID = value; } } + public override bool PIDActive + { + set + { + AddChange(changes.PIDActive,value); + } + } + public override float PIDTau { set { - if (value <= 0) - m_PIDTau = 0; - else + float tmp = 0; + if (value > 0) { float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); if (value < mint) - m_PIDTau = mint; + tmp = mint; else - m_PIDTau = value; + tmp = value; } + AddChange(changes.PIDTau,tmp); } } @@ -720,27 +718,39 @@ namespace OpenSim.Region.Physics.OdePlugin { set { - m_PIDHoverHeight = value; - if (value == 0) - m_useHoverPID = false; + AddChange(changes.PIDHoverHeight,value); + } + } + public override bool PIDHoverActive + { + set + { + AddChange(changes.PIDHoverActive, value); } } - public override bool PIDHoverActive { set { m_useHoverPID = value; } } - public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + + public override PIDHoverType PIDHoverType + { + set + { + AddChange(changes.PIDHoverType,value); + } + } + public override float PIDHoverTau { set { - if (value <= 0) - m_PIDHoverTau = 0; - else + float tmp =0; + if (value > 0) { float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); if (value < mint) - m_PIDHoverTau = mint; + tmp = mint; else - m_PIDHoverTau = value; + tmp = value; } + AddChange(changes.PIDHoverTau, tmp); } } @@ -981,7 +991,6 @@ namespace OpenSim.Region.Physics.OdePlugin return false; } - public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) { @@ -3099,7 +3108,6 @@ namespace OpenSim.Region.Physics.OdePlugin resetCollisionAccounting(); } - private void changeDisable(bool disable) { if (disable) @@ -3250,7 +3258,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(Body); } - private void changeAddForce(Vector3 theforce) { m_forceacc += theforce; @@ -3267,7 +3274,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(Body); } } - m_collisionscore = 0; } } @@ -3317,7 +3323,6 @@ namespace OpenSim.Region.Physics.OdePlugin _velocity = newVel; } - private void changeangvelocity(Vector3 newAngVel) { float len = newAngVel.LengthSquared(); @@ -3384,6 +3389,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_vehicle = new ODEDynamics(this); m_vehicle.DoSetVehicle(vdata); } + private void changeVehicleType(int value) { if (value == (int)Vehicle.TYPE_NONE) @@ -3429,6 +3435,48 @@ namespace OpenSim.Region.Physics.OdePlugin m_vehicle.ProcessVehicleFlags(bp.param, bp.value); } + private void changeBuoyancy(float b) + { + m_buoyancy = b; + } + + private void changePIDTarget(Vector3 trg) + { + m_PIDTarget = trg; + } + + private void changePIDTau(float tau) + { + m_PIDTau = tau; + } + + private void changePIDActive(bool val) + { + m_usePID = val; + } + + private void changePIDHoverHeight(float val) + { + m_PIDHoverHeight = val; + if (val == 0) + m_useHoverPID = false; + } + + private void changePIDHoverType(PIDHoverType type) + { + m_PIDHoverType = type; + } + + private void changePIDHoverTau(float tau) + { + m_PIDHoverTau = tau; + } + + private void changePIDHoverActive(bool active) + { + m_useHoverPID = active; + } + #endregion public void Move() @@ -3987,6 +4035,39 @@ namespace OpenSim.Region.Physics.OdePlugin case changes.SetVehicle: changeSetVehicle((VehicleData) arg); break; + + case changes.Buoyancy: + changeBuoyancy((float)arg); + break; + + case changes.PIDTarget: + changePIDTarget((Vector3)arg); + break; + + case changes.PIDTau: + changePIDTau((float)arg); + break; + + case changes.PIDActive: + changePIDActive((bool)arg); + break; + + case changes.PIDHoverHeight: + changePIDHoverHeight((float)arg); + break; + + case changes.PIDHoverType: + changePIDHoverType((PIDHoverType)arg); + break; + + case changes.PIDHoverTau: + changePIDHoverTau((float)arg); + break; + + case changes.PIDHoverActive: + changePIDHoverActive((bool)arg); + break; + case changes.Null: donullchange(); break; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 3ee5198..f126644 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -134,6 +134,17 @@ namespace OpenSim.Region.Physics.OdePlugin AddAngForce, AngLock, + Buoyancy, + + PIDTarget, + PIDTau, + PIDActive, + + PIDHoverHeight, + PIDHoverType, + PIDHoverTau, + PIDHoverActive, + Size, Shape, -- cgit v1.1 From 7137b234b4e99d8d19e83dfb5268674c72f46479 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 3 Oct 2012 19:33:28 +0100 Subject: introduce a ODEMeshWorker class, should be pure cosmetic changes for now --- OpenSim/Region/Physics/Manager/IMesher.cs | 3 +- OpenSim/Region/Physics/Manager/ZeroMesher.cs | 6 + OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 5 + OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 40 ++++- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 199 +++++++++++++++++++++ OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 166 +---------------- 7 files changed, 262 insertions(+), 163 deletions(-) create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index a8c99f7..06f3c8b 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -37,7 +37,8 @@ namespace OpenSim.Region.Physics.Manager { IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod); IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical); - IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical,bool convex); + IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex); + IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex); void ReleaseMesh(IMesh mesh); void ExpireReleaseMeshs(); } diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs index f555cb9..61da9f3 100644 --- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs +++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs @@ -79,6 +79,12 @@ namespace OpenSim.Region.Physics.Manager return null; } + + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + return null; + } + public void ReleaseMesh(IMesh mesh) { } public void ExpireReleaseMeshs() { } } diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index 3c4f737..143648e 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -763,6 +763,11 @@ namespace OpenSim.Region.Physics.Meshing return mesh; } + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + return null; + } + public void ReleaseMesh(IMesh imesh) { } public void ExpireReleaseMeshs() { } } diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 4c40175..44c8972 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -1061,6 +1061,42 @@ namespace OpenSim.Region.Physics.Meshing private static Vector3 m_MeshUnitSize = new Vector3(0.5f, 0.5f, 0.5f); + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + Mesh mesh = null; + + if (size.X < 0.01f) size.X = 0.01f; + if (size.Y < 0.01f) size.Y = 0.01f; + if (size.Z < 0.01f) size.Z = 0.01f; + + AMeshKey key = GetMeshUniqueKey(primShape, size, (byte)lod, convex); + lock (m_uniqueMeshes) + { + m_uniqueMeshes.TryGetValue(key, out mesh); + + if (mesh != null) + { + mesh.RefCount++; + return mesh; + } + } + + // try to find a identical mesh on meshs recently released + lock (m_uniqueReleasedMeshes) + { + m_uniqueReleasedMeshes.TryGetValue(key, out mesh); + if (mesh != null) + { + m_uniqueReleasedMeshes.Remove(key); + lock (m_uniqueMeshes) + m_uniqueMeshes.Add(key, mesh); + mesh.RefCount = 1; + return mesh; + } + } + return null; + } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) { #if SPAM @@ -1068,15 +1104,13 @@ namespace OpenSim.Region.Physics.Meshing #endif Mesh mesh = null; -// ulong key = 0; - if (size.X < 0.01f) size.X = 0.01f; if (size.Y < 0.01f) size.Y = 0.01f; if (size.Z < 0.01f) size.Z = 0.01f; // try to find a identical mesh on meshs in use -// key = primShape.GetMeshKey(size, lod, convex); + AMeshKey key = GetMeshUniqueKey(primShape,size,(byte)lod, convex); lock (m_uniqueMeshes) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs new file mode 100644 index 0000000..81d59a6 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -0,0 +1,199 @@ +/* + * AJLDuarte 2012 + */ + +using System; +using System.Threading; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OdeAPI; +using log4net; +using Nini.Config; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEMeshWorker + { + private ILog m_log; + private OdeScene m_scene; + private IMesher m_mesher; + + public bool meshSculptedPrim = true; + public bool forceSimplePrimMeshing = false; + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 32; + + + public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig) + { + m_scene = pScene; + m_log = pLog; + m_mesher = pMesher; + + if (pConfig != null) + { + forceSimplePrimMeshing = pConfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); + meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim); + meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD); + MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); + } + } + + + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// + public bool needsMeshing(PrimitiveBaseShape pbs) + { + // check sculpts or meshs + if (pbs.SculptEntry) + { + if (meshSculptedPrim) + return true; + + if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs + return true; + + return false; + } + + if (forceSimplePrimMeshing) + return true; + + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim + + if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) + || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 + && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) + { + + if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 + && pbs.ProfileHollow == 0 + && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 + && pbs.PathBegin == 0 && pbs.PathEnd == 0 + && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 + && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 + && pbs.PathShearX == 0 && pbs.PathShearY == 0) + { + return false; + } + } + + // following code doesn't give meshs to boxes and spheres ever + // and it's odd.. so for now just return true if asked to force meshs + // hopefully mesher will fail if doesn't suport so things still get basic boxes + + int iPropertiesNotSupportedDefault = 0; + + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + + if (iPropertiesNotSupportedDefault == 0) + { + return false; + } + return true; + } + + public IMesh getMesh(PhysicsActor actor, PrimitiveBaseShape ppbs, Vector3 psize, byte pshapetype) + { + if (!(actor is OdePrim)) + return null; + + IMesh mesh = null; + PrimitiveBaseShape pbs = ppbs; + Vector3 size = psize; + byte shapetype = pshapetype; + + if (needsMeshing(pbs) && (pbs.SculptData.Length > 0)) + { + bool convex; + int clod = (int)LevelOfDetail.High; + if (shapetype == 0) + convex = false; + else + { + convex = true; + if (pbs.SculptType != (byte)SculptType.Mesh) + clod = (int)LevelOfDetail.Low; + } + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + if(mesh == null) + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + } + return mesh; + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index f2f4725..545bd27 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1080,7 +1080,7 @@ namespace OpenSim.Region.Physics.OdePlugin bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; CalcPrimBodyData(); - +/* m_mesh = null; if (_parent_scene.needsMeshing(pbs) && (pbs.SculptData.Length > 0)) { @@ -1096,6 +1096,8 @@ namespace OpenSim.Region.Physics.OdePlugin } m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, clod, true, convex); } +*/ + m_mesh = _parent_scene.m_meshWorker.getMesh(this, pbs, _size, m_shapetype); m_building = true; // control must set this to false when done @@ -1476,7 +1478,7 @@ namespace OpenSim.Region.Physics.OdePlugin hasOOBoffsetFromMesh = false; m_NoColide = false; - if (_parent_scene.needsMeshing(_pbs)) + if (_parent_scene.m_meshWorker.needsMeshing(_pbs)) { haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims if (!haveMesh) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index f126644..7c00600 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -230,11 +230,6 @@ namespace OpenSim.Region.Physics.OdePlugin private float minimumGroundFlightOffset = 3f; public float maximumMassObject = 10000.01f; - public bool meshSculptedPrim = true; - public bool forceSimplePrimMeshing = false; - - public float meshSculptLOD = 32; - public float MeshSculptphysicalLOD = 32; public float geomDefaultDensity = 10.000006836f; @@ -328,7 +323,7 @@ namespace OpenSim.Region.Physics.OdePlugin private PhysicsScene m_parentScene = null; private ODERayCastRequestManager m_rayCastManager; - + public ODEMeshWorker m_meshWorker; /* maybe needed if ode uses tls private void checkThread() @@ -361,6 +356,8 @@ namespace OpenSim.Region.Physics.OdePlugin nearCallback = near; m_rayCastManager = new ODERayCastRequestManager(this); + + lock (OdeLock) { // Create the world and the first space @@ -440,9 +437,11 @@ namespace OpenSim.Region.Physics.OdePlugin int contactsPerCollision = 80; + IConfig physicsconfig = null; + if (m_config != null) { - IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + physicsconfig = m_config.Configs["ODEPhysicsSettings"]; if (physicsconfig != null) { gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); @@ -469,27 +468,7 @@ namespace OpenSim.Region.Physics.OdePlugin geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); -/* - bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD); - bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG); -*/ - forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); - meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim); - meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD); - MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); -/* - if (Environment.OSVersion.Platform == PlatformID.Unix) - { - avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD); - avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", avPIDP); - } - else - { - - avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD); - avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP); - } -*/ + physics_logging = physicsconfig.GetBoolean("physics_logging", false); physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); @@ -499,6 +478,8 @@ namespace OpenSim.Region.Physics.OdePlugin } } + m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); + HalfOdeStep = ODE_STEPSIZE * 0.5f; odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); @@ -1615,135 +1596,6 @@ namespace OpenSim.Region.Physics.OdePlugin #endregion - /// - /// Routine to figure out if we need to mesh this prim with our mesher - /// - /// - /// - public bool needsMeshing(PrimitiveBaseShape pbs) - { - // check sculpts or meshs - if (pbs.SculptEntry) - { - if (meshSculptedPrim) - return true; - - if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs - return true; - - return false; - } - - if (forceSimplePrimMeshing) - return true; - - // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim - - if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) - || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 - && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) - { - - if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 - && pbs.ProfileHollow == 0 - && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 - && pbs.PathBegin == 0 && pbs.PathEnd == 0 - && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 - && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 - && pbs.PathShearX == 0 && pbs.PathShearY == 0) - { -#if SPAM - m_log.Warn("NonMesh"); -#endif - return false; - } - } - - // following code doesn't give meshs to boxes and spheres ever - // and it's odd.. so for now just return true if asked to force meshs - // hopefully mesher will fail if doesn't suport so things still get basic boxes - - int iPropertiesNotSupportedDefault = 0; - - if (pbs.ProfileHollow != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) - iPropertiesNotSupportedDefault++; - - if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) - iPropertiesNotSupportedDefault++; - - // test for torus - if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) - { - if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) - { - if (pbs.PathCurve == (byte)Extrusion.Straight) - { - iPropertiesNotSupportedDefault++; - } - - // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits - else if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) - { - if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) - { - if (pbs.PathCurve == (byte)Extrusion.Straight) - { - iPropertiesNotSupportedDefault++; - } - else if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - - if (iPropertiesNotSupportedDefault == 0) - { -#if SPAM - m_log.Warn("NonMesh"); -#endif - return false; - } -#if SPAM - m_log.Debug("Mesh"); -#endif - return true; - } /// /// Called to queue a change to a actor -- cgit v1.1 From 4f51cc325c627e9e9c1381e6e813a266968895d4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 3 Oct 2012 20:36:41 +0100 Subject: making meshworker have more work.. --- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 24 ++++++++++++++++++---- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 3 +++ 2 files changed, 23 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 81d59a6..f57149c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -5,6 +5,7 @@ using System; using System.Threading; using System.Collections.Generic; +using System.IO; using System.Reflection; using System.Runtime.InteropServices; using System.Text; @@ -177,7 +178,7 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 size = psize; byte shapetype = pshapetype; - if (needsMeshing(pbs) && (pbs.SculptData.Length > 0)) + if (needsMeshing(pbs)) { bool convex; int clod = (int)LevelOfDetail.High; @@ -189,9 +190,24 @@ namespace OpenSim.Region.Physics.OdePlugin if (pbs.SculptType != (byte)SculptType.Mesh) clod = (int)LevelOfDetail.Low; } - mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); - if(mesh == null) - mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); + if (mesh == null) + { + if (!pbs.SculptEntry) + return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + + if (pbs.SculptTexture == UUID.Zero) + return null; + + if(pbs.SculptType != (byte)SculptType.Mesh) + { // check for sculpt decoded image on cache) + if (File.Exists(System.IO.Path.Combine("j2kDecodeCache", "smap_" + pbs.SculptTexture.ToString()))) + return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + } + + if(pbs.SculptData != null && pbs.SculptData.Length >0) + return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + } } return mesh; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 545bd27..f328066 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1353,6 +1353,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_mesh == null) { +/* bool convex; int clod = (int)LevelOfDetail.High; @@ -1366,6 +1367,8 @@ namespace OpenSim.Region.Physics.OdePlugin } mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, clod, true, convex); +*/ + mesh = _parent_scene.m_meshWorker.getMesh(this, _pbs, _size, m_shapetype); } else { -- cgit v1.1 From 9988558ec16144f1a69b666d39428cda4c0849c3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 3 Oct 2012 23:14:56 +0100 Subject: meshworker basic replacement of SOP CheckSculptAndLoad ( for now disabled for all physics engines) --- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 43 ++++++++++++++++++++-- 1 file changed, 39 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index f57149c..b0231e9 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -29,12 +29,11 @@ namespace OpenSim.Region.Physics.OdePlugin public float meshSculptLOD = 32; public float MeshSculptphysicalLOD = 32; - public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig) { m_scene = pScene; m_log = pLog; - m_mesher = pMesher; + m_mesher = pMesher; if (pConfig != null) { @@ -45,8 +44,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - - /// /// Routine to figure out if we need to mesh this prim with our mesher /// @@ -207,9 +204,47 @@ namespace OpenSim.Region.Physics.OdePlugin if(pbs.SculptData != null && pbs.SculptData.Length >0) return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + + ODEAssetRequest asr; + RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; + if (assetProvider != null) + asr = new ODEAssetRequest(this, assetProvider, actor, pbs); + return null; } } return mesh; } } + + public class ODEAssetRequest + { + PhysicsActor m_actor; + ODEMeshWorker m_worker; + PrimitiveBaseShape m_pbs; + + public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider, PhysicsActor pActor, PrimitiveBaseShape ppbs) + { + m_actor = pActor; + m_worker = pWorker; + m_pbs = ppbs; + + if (provider == null) + return; + + UUID assetID = m_pbs.SculptTexture; + if (assetID == UUID.Zero) + return; + + provider(assetID, ODEassetReceived); + } + + void ODEassetReceived(AssetBase asset) + { + if (m_actor != null && m_pbs != null && asset != null && asset.Data != null && asset.Data.Length > 0) + { + m_pbs.SculptData = asset.Data; + m_actor.Shape = m_pbs; + } + } + } } \ No newline at end of file -- cgit v1.1 From a9f2bc150f8177e9e6340b7591b2f846f0678329 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 3 Oct 2012 23:18:35 +0100 Subject: missing changed file --- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 5274f3b..46bfa59 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -106,7 +106,7 @@ namespace OpenSim.Region.Physics.Manager get { return new NullPhysicsScene(); } } - public RequestAssetDelegate RequestAssetMethod { private get; set; } + public RequestAssetDelegate RequestAssetMethod { get; set; } public virtual void TriggerPhysicsBasedRestart() { -- cgit v1.1 From 51e1830f86cd0253879f4f60470a3b5fa2b3db7c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 4 Oct 2012 04:55:53 +0100 Subject: more changes. Most code not in use --- OpenSim/Region/Physics/Manager/IMesher.cs | 1 + OpenSim/Region/Physics/Meshing/Mesh.cs | 6 + OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 78 ++- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 521 ++++++++++++++++++++- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 32 +- 5 files changed, 615 insertions(+), 23 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index 06f3c8b..460b48e 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -82,5 +82,6 @@ namespace OpenSim.Region.Physics.Manager void Append(IMesh newMesh); void TransformLinear(float[,] matrix, float[] offset); Vector3 GetCentroid(); + Vector3 GetOBB(); } } diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs index c715642..c03f18b 100644 --- a/OpenSim/Region/Physics/Meshing/Mesh.cs +++ b/OpenSim/Region/Physics/Meshing/Mesh.cs @@ -148,6 +148,12 @@ namespace OpenSim.Region.Physics.Meshing return Vector3.Zero; } + // not functional + public Vector3 GetOBB() + { + return new Vector3(0.5f, 0.5f, 0.5f); + } + public void CalcNormals() { int iTriangles = m_triangles.Count; diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index 98c0f0b..47e0cf4 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -48,8 +48,14 @@ namespace OpenSim.Region.Physics.Meshing int m_indexCount = 0; public float[] m_normals; Vector3 m_centroid; - int m_centroidDiv; + float m_obbXmin; + float m_obbXmax; + float m_obbYmin; + float m_obbYmax; + float m_obbZmin; + float m_obbZmax; + int m_centroidDiv; private class vertexcomp : IEqualityComparer { @@ -77,6 +83,14 @@ namespace OpenSim.Region.Physics.Meshing m_triangles = new List(); m_centroid = Vector3.Zero; m_centroidDiv = 0; + m_obbXmin = float.MaxValue; + m_obbXmax = float.MinValue; + m_obbYmin = float.MaxValue; + m_obbYmax = float.MinValue; + m_obbZmin = float.MaxValue; + m_obbZmax = float.MinValue; + + } public int RefCount { get; set; } @@ -102,6 +116,35 @@ namespace OpenSim.Region.Physics.Meshing return result; } + public void addVertexLStats(Vertex v) + { + float x = v.X; + float y = v.Y; + float z = v.Z; + + m_centroid.X += x; + m_centroid.Y += y; + m_centroid.Z += z; + m_centroidDiv++; + + if (x > m_obbXmax) + m_obbXmax = x; + else if (x < m_obbXmin) + m_obbXmin = x; + + if (y > m_obbYmax) + m_obbYmax = y; + else if (y < m_obbYmin) + m_obbYmin = y; + + if (z > m_obbZmax) + m_obbZmax = z; + else if (z < m_obbZmin) + m_obbZmin = z; + + } + + public void Add(Triangle triangle) { if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) @@ -127,26 +170,17 @@ namespace OpenSim.Region.Physics.Meshing if (!m_vertices.ContainsKey(triangle.v1)) { m_vertices[triangle.v1] = m_vertices.Count; - m_centroid.X += triangle.v1.X; - m_centroid.Y += triangle.v1.Y; - m_centroid.Z += triangle.v1.Z; - m_centroidDiv++; + addVertexLStats(triangle.v1); } if (!m_vertices.ContainsKey(triangle.v2)) { m_vertices[triangle.v2] = m_vertices.Count; - m_centroid.X += triangle.v2.X; - m_centroid.Y += triangle.v2.Y; - m_centroid.Z += triangle.v2.Z; - m_centroidDiv++; + addVertexLStats(triangle.v2); } if (!m_vertices.ContainsKey(triangle.v3)) { m_vertices[triangle.v3] = m_vertices.Count; - m_centroid.X += triangle.v3.X; - m_centroid.Y += triangle.v3.Y; - m_centroid.Z += triangle.v3.Z; - m_centroidDiv++; + addVertexLStats(triangle.v3); } m_triangles.Add(triangle); } @@ -159,6 +193,24 @@ namespace OpenSim.Region.Physics.Meshing return Vector3.Zero; } + public Vector3 GetOBB() + { + float x, y, z; + if (m_centroidDiv > 0) + { + x = (m_obbXmax - m_obbXmin) * 0.5f; + y = (m_obbYmax - m_obbYmin) * 0.5f; + z = (m_obbZmax - m_obbZmin) * 0.5f; + } + else // ?? + { + x = 0.5f; + y = 0.5f; + z = 0.5f; + } + return new Vector3(x, y, z); + } + public void CalcNormals() { int iTriangles = m_triangles.Count; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index b0231e9..9bf3667 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -24,16 +24,20 @@ namespace OpenSim.Region.Physics.OdePlugin private OdeScene m_scene; private IMesher m_mesher; + public bool meshSculptedPrim = true; public bool forceSimplePrimMeshing = false; public float meshSculptLOD = 32; public float MeshSculptphysicalLOD = 32; - public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig) + private IntPtr m_workODEspace = IntPtr.Zero; + + public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IntPtr pWorkSpace, IConfig pConfig) { m_scene = pScene; m_log = pLog; - m_mesher = pMesher; + m_mesher = pMesher; + m_workODEspace = pWorkSpace; if (pConfig != null) { @@ -196,24 +200,508 @@ namespace OpenSim.Region.Physics.OdePlugin if (pbs.SculptTexture == UUID.Zero) return null; - if(pbs.SculptType != (byte)SculptType.Mesh) + if (pbs.SculptType != (byte)SculptType.Mesh) { // check for sculpt decoded image on cache) if (File.Exists(System.IO.Path.Combine("j2kDecodeCache", "smap_" + pbs.SculptTexture.ToString()))) return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); } - if(pbs.SculptData != null && pbs.SculptData.Length >0) + if (pbs.SculptData != null && pbs.SculptData.Length > 0) return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); ODEAssetRequest asr; RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; if (assetProvider != null) - asr = new ODEAssetRequest(this, assetProvider, actor, pbs); + asr = new ODEAssetRequest(this, assetProvider, actor, pbs, m_log); + return null; } } return mesh; } + + private bool GetTriMeshGeo(ODEPhysRepData repData) + { + IntPtr vertices, indices; + IntPtr triMeshData = IntPtr.Zero; + IntPtr geo = IntPtr.Zero; + int vertexCount, indexCount; + int vertexStride, triStride; + + PhysicsActor actor = repData.actor; + + IMesh mesh = repData.mesh; + + if (mesh == null) + { + mesh = getMesh(repData.actor, repData.pbs, repData.size, repData.shapetype); + } + + if (mesh == null) + return false; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}", + actor.Name, repData.pbs.SculptTexture.ToString()); + mesh.releaseSourceMeshData(); + return false; + } + + repData.OBBOffset = mesh.GetCentroid(); + repData.OBB = mesh.GetOBB(); + repData.hasOBB = true; + repData.physCost = 0.0013f * (float)indexCount; + + mesh.releaseSourceMeshData(); + + try + { + triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(triMeshData); + + m_scene.waitForSpaceUnlock(m_workODEspace); + geo = d.CreateTriMesh(m_workODEspace, triMeshData, null, null, null); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", actor.Name, e); + if (triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(triMeshData); + repData.triMeshData = IntPtr.Zero; + } + repData.geo = IntPtr.Zero; + return false; + } + + repData.geo = geo; + repData.triMeshData = triMeshData; + repData.curSpace = m_workODEspace; + return true; + } + + public ODEPhysRepData CreateActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, IMesh pMesh, Vector3 size, byte shapetype) + { + ODEPhysRepData repData = new ODEPhysRepData(); + + repData.actor = actor; + repData.pbs = pbs; + repData.mesh = pMesh; + repData.size = size; + repData.shapetype = shapetype; + + IntPtr geo = IntPtr.Zero; + bool hasMesh = false; + if (needsMeshing(pbs)) + { + if (GetTriMeshGeo(repData)) + hasMesh = true; + else + repData.canColide = false; + } + + if (!hasMesh) + { + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 + && size.X == size.Y && size.Y == size.Z) + { // it's a sphere + m_scene.waitForSpaceUnlock(m_workODEspace); + try + { + geo = d.CreateSphere(m_workODEspace, size.X * 0.5f); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return null; + } + } + else + {// do it as a box + m_scene.waitForSpaceUnlock(m_workODEspace); + try + { + //Console.WriteLine(" CreateGeom 4"); + geo = d.CreateBox(m_workODEspace, size.X, size.Y, size.Z); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return null; + } + } + + repData.physCost = 0.1f; + repData.streamCost = 1.0f; + repData.geo = geo; + } + + repData.curSpace = m_workODEspace; + + CalcVolumeData(repData); + + return repData; + } + + private void CalculateBasicPrimVolume(ODEPhysRepData repData) + { + PrimitiveBaseShape _pbs = repData.pbs; + Vector3 _size = repData.size; + + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.5236f; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Circle: + case HollowShape.Triangle: // diference in sl is minor and odd + case HollowShape.Same: + break; + + case HollowShape.Square: + hollowVolume *= 0.909f; + break; + + // case HollowShape.Triangle: + // hollowVolume *= .827f; + // break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + repData.volume = volume; + } + + private void CalcVolumeData(ODEPhysRepData repData) + { + float volume; + Vector3 OBB = repData.size; + Vector3 OBBoffset; + IntPtr geo = repData.geo; + + if (geo == IntPtr.Zero || repData.triMeshData == IntPtr.Zero) + { + OBB.X *= 0.5f; + OBB.Y *= 0.5f; + OBB.Z *= 0.5f; + + repData.OBB = OBB; + repData.OBBOffset = Vector3.Zero; + } + else if (!repData.hasOBB) // should this happen? + { + d.AABB AABB; + d.GeomGetAABB(geo, out AABB); // get the AABB from engine geom + + OBB.X = (AABB.MaxX - AABB.MinX) * 0.5f; + OBB.Y = (AABB.MaxY - AABB.MinY) * 0.5f; + OBB.Z = (AABB.MaxZ - AABB.MinZ) * 0.5f; + repData.OBB = OBB; + OBBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; + OBBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; + OBBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; + repData.OBBOffset = Vector3.Zero; + } + + // also its own inertia and mass + // keep using basic shape mass for now + CalculateBasicPrimVolume(repData); + + if (repData.hasOBB) + { + OBB = repData.OBB; + float pc = repData.physCost; + float psf = OBB.X * (OBB.Y + OBB.Z) + OBB.Y * OBB.Z; + psf *= 1.33f * .2f; + + pc *= psf; + if (pc < 0.1f) + pc = 0.1f; + + repData.physCost = pc; + } + else + repData.physCost = 0.1f; + } } public class ODEAssetRequest @@ -221,12 +709,15 @@ namespace OpenSim.Region.Physics.OdePlugin PhysicsActor m_actor; ODEMeshWorker m_worker; PrimitiveBaseShape m_pbs; + private ILog m_log; - public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider, PhysicsActor pActor, PrimitiveBaseShape ppbs) + public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider, + PhysicsActor pActor, PrimitiveBaseShape ppbs, ILog plog) { m_actor = pActor; m_worker = pWorker; m_pbs = ppbs; + m_log = plog; if (provider == null) return; @@ -240,10 +731,22 @@ namespace OpenSim.Region.Physics.OdePlugin void ODEassetReceived(AssetBase asset) { - if (m_actor != null && m_pbs != null && asset != null && asset.Data != null && asset.Data.Length > 0) + if (m_actor != null && m_pbs != null) { - m_pbs.SculptData = asset.Data; - m_actor.Shape = m_pbs; + if (asset != null) + { + if (asset.Data != null && asset.Data.Length > 0) + { + m_pbs.SculptData = asset.Data; + m_actor.Shape = m_pbs; + } + else + m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.", + m_actor.Name, asset.ID.ToString()); + } + else + m_log.WarnFormat("[PHYSICS]: asset provider returned null asset fo mesh of prim {0}.", + m_actor.Name); } } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 7c00600..d426112 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -60,6 +60,29 @@ namespace OpenSim.Region.Physics.OdePlugin public int lastframe; } + public class ODEPhysRepData + { + public PhysicsActor actor; + public IntPtr geo = IntPtr.Zero; + public IntPtr triMeshData = IntPtr.Zero; + public IMesh mesh; + public IntPtr curSpace = IntPtr.Zero; + public PrimitiveBaseShape pbs; + + public Vector3 size = Vector3.Zero; + public Vector3 OBB = Vector3.Zero; + public Vector3 OBBOffset = Vector3.Zero; + + public float volume; + + public float physCost = 0.0f; + public float streamCost = 0; + public byte shapetype = 0; + public bool canColide = true; + public bool hasOBB = false; + public bool hasMeshVolume = false; + } + // colision flags of things others can colide with // rays, sensors, probes removed since can't be colided with // The top space where things are placed provided further selection @@ -297,6 +320,7 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr TopSpace; // the global space public IntPtr ActiveSpace; // space for active prims public IntPtr StaticSpace; // space for the static things around + public IntPtr WorkSpace; // no collisions work space // some speedup variables private int spaceGridMaxX; @@ -369,6 +393,7 @@ namespace OpenSim.Region.Physics.OdePlugin // now the major subspaces ActiveSpace = d.HashSpaceCreate(TopSpace); StaticSpace = d.HashSpaceCreate(TopSpace); + WorkSpace = d.HashSpaceCreate(TopSpace); } catch { @@ -378,10 +403,12 @@ namespace OpenSim.Region.Physics.OdePlugin d.HashSpaceSetLevels(TopSpace, -2, 8); d.HashSpaceSetLevels(ActiveSpace, -2, 8); d.HashSpaceSetLevels(StaticSpace, -2, 8); + d.HashSpaceSetLevels(WorkSpace, -2, 8); // demote to second level d.SpaceSetSublevel(ActiveSpace, 1); d.SpaceSetSublevel(StaticSpace, 1); + d.SpaceSetSublevel(WorkSpace, 1); d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | CollisionCategories.Geom | @@ -399,6 +426,9 @@ namespace OpenSim.Region.Physics.OdePlugin )); d.GeomSetCollideBits(StaticSpace, 0); + d.GeomSetCategoryBits(WorkSpace, 0); + d.GeomSetCollideBits(WorkSpace, 0); + contactgroup = d.JointGroupCreate(0); //contactgroup @@ -478,7 +508,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); + m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, WorkSpace, physicsconfig); HalfOdeStep = ODE_STEPSIZE * 0.5f; odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); -- cgit v1.1 From 89d342b5ce9ac5d9fc4fd493eead8e050f99c91a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 4 Oct 2012 08:14:52 +0100 Subject: more changes and more non active code --- OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 10 +- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 11 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 140 +++++++++++++++++---- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 12 +- 4 files changed, 144 insertions(+), 29 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index 47e0cf4..c31ec08 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -89,8 +89,6 @@ namespace OpenSim.Region.Physics.Meshing m_obbYmax = float.MinValue; m_obbZmin = float.MaxValue; m_obbZmax = float.MinValue; - - } public int RefCount { get; set; } @@ -99,8 +97,6 @@ namespace OpenSim.Region.Physics.Meshing public void Scale(Vector3 scale) { - - } public Mesh Clone() @@ -113,6 +109,12 @@ namespace OpenSim.Region.Physics.Meshing } result.m_centroid = m_centroid; result.m_centroidDiv = m_centroidDiv; + result.m_obbXmin = m_obbXmin; + result.m_obbXmax = m_obbXmax; + result.m_obbYmin = m_obbYmin; + result.m_obbYmax = m_obbYmax; + result.m_obbZmin = m_obbZmin; + result.m_obbZmax = m_obbZmax; return result; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 9bf3667..702c336 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -304,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (GetTriMeshGeo(repData)) hasMesh = true; else - repData.canColide = false; + repData.NoColide = true; } if (!hasMesh) @@ -350,6 +350,15 @@ namespace OpenSim.Region.Physics.OdePlugin return repData; } + public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, + Vector3 size, byte shapetype, MeshWorkerChange what) + { + ODEPhysRepData repData = CreateActorPhysRep(actor, pbs, null, size, shapetype); + repData.changed |= what; + if (repData != null && actor != null) + ((OdePrim)actor).AddChange(changes.PhysRepData, repData); + } + private void CalculateBasicPrimVolume(ODEPhysRepData repData) { PrimitiveBaseShape _pbs = repData.pbs; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index f328066..7650571 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -104,9 +104,6 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_PIDTau; private bool m_usePID; - // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), - // and are for non-VEHICLES only. - private float m_PIDHoverHeight; private float m_PIDHoverTau; private bool m_useHoverPID; @@ -395,6 +392,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { AddChange(changes.Size, value); + +// _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype, MeshWorkerChange.size); } else { @@ -529,6 +528,7 @@ namespace OpenSim.Region.Physics.OdePlugin set { AddChange(changes.Shape, value); +// _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype, MeshWorkerChange.shape); } } @@ -542,10 +542,10 @@ namespace OpenSim.Region.Physics.OdePlugin { m_shapetype = value; AddChange(changes.Shape, null); +// _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value, MeshWorkerChange.shapetype); } } - public override Vector3 Velocity { get @@ -1529,7 +1529,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prim_geom != IntPtr.Zero) { -// _parent_scene.geom_name_map.Remove(prim_geom); _parent_scene.actor_name_map.Remove(prim_geom); try { @@ -1539,11 +1538,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomTriMeshDataDestroy(_triMeshData); _triMeshData = IntPtr.Zero; } - } - - - // catch (System.AccessViolationException) catch (Exception e) { m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); @@ -1557,23 +1552,19 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); } - if (m_mesh != null) + lock (m_meshlock) { - _parent_scene.mesher.ReleaseMesh(m_mesh); - m_mesh = null; + if (m_mesh != null) + { + _parent_scene.mesher.ReleaseMesh(m_mesh); + m_mesh = null; + } } Body = IntPtr.Zero; hasOOBoffsetFromMesh = false; } -/* - private void ChildSetGeom(OdePrim odePrim) - { - // well.. - DestroyBody(); - MakeBody(); - } -*/ + //sets non physical prim m_targetSpace to right space in spaces grid for static prims // should only be called for non physical prims unless they are becoming non physical private void SetInStaticSpace(OdePrim prim) @@ -1636,9 +1627,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) { -// d.BodyDestroy(Body); -// Body = IntPtr.Zero; - // do a more complet destruction DestroyBody(); m_log.Warn("[PHYSICS]: MakeBody called having a body"); } @@ -2500,6 +2488,26 @@ namespace OpenSim.Region.Physics.OdePlugin primOOBradiusSQ = primOOBsize.LengthSquared(); } + private void UpdatePrimBodyData() + { + primMass = m_density * primVolume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + _mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); + + d.MassTranslate(ref primdMass, + primOOBoffset.X, + primOOBoffset.Y, + primOOBoffset.Z); + + primOOBradiusSQ = primOOBsize.LengthSquared(); + } #endregion @@ -3232,6 +3240,86 @@ namespace OpenSim.Region.Physics.OdePlugin changeprimsizeshape(); } + + private void changePhysRepData(ODEPhysRepData repData) + { + CheckDelaySelect(); + + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + prim_geom = repData.geo; + _triMeshData = repData.triMeshData; + _size = repData.size; + _pbs = repData.pbs; + m_mesh = repData.mesh; + m_shapetype = repData.shapetype; + + hasOOBoffsetFromMesh = repData.hasOBB; + primOOBoffset = repData.OBBOffset; + primOOBsize = repData.OBB; + + m_NoColide = repData.NoColide; +// m_physCost = repData.physCost; +// m_streamCost = repData.streamCost; + + primVolume = repData.volume; + m_targetSpace = repData.curSpace; + + UpdatePrimBodyData(); + + _parent_scene.actor_name_map[prim_geom] = this; + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (m_isphysical) + { + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + } + + else + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + + resetCollisionAccounting(); + } + + private void changeFloatOnWater(bool newval) { m_collidesWater = newval; @@ -3989,6 +4077,10 @@ namespace OpenSim.Region.Physics.OdePlugin changeShape((PrimitiveBaseShape)arg); break; + case changes.PhysRepData: + changePhysRepData((ODEPhysRepData) arg); + break; + case changes.CollidesWater: changeFloatOnWater((bool)arg); break; @@ -4077,6 +4169,8 @@ namespace OpenSim.Region.Physics.OdePlugin donullchange(); break; + + default: donullchange(); break; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index d426112..d758c85 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -77,8 +77,9 @@ namespace OpenSim.Region.Physics.OdePlugin public float physCost = 0.0f; public float streamCost = 0; + public MeshWorkerChange changed; public byte shapetype = 0; - public bool canColide = true; + public bool NoColide = false; public bool hasOBB = false; public bool hasMeshVolume = false; } @@ -132,6 +133,14 @@ namespace OpenSim.Region.Physics.OdePlugin light = 7 // compatibility with old viewers } + [Flags] + public enum MeshWorkerChange : uint + { + none = 0, + size = 1, + shape = 2, + shapetype = 3, + } public enum changes : int { @@ -170,6 +179,7 @@ namespace OpenSim.Region.Physics.OdePlugin Size, Shape, + PhysRepData, CollidesWater, VolumeDtc, -- cgit v1.1 From 78ce7a0a04dc5ce3212acfb0e88a3a5a1b876100 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 7 Oct 2012 01:20:52 +0100 Subject: [DANGER UNTESTED] ODE mesh assets. Other plugins will not do meshs/sculpts now --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 64 +- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 1 - .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 560 +++++++++----- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 830 +++++---------------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 187 ++--- 5 files changed, 609 insertions(+), 1033 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 44c8972..2933d86 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -74,10 +74,6 @@ namespace OpenSim.Region.Physics.Meshing private const string baseDir = null; //"rawFiles"; #endif - private bool cacheSculptMaps = true; - private bool cacheSculptAlphaMaps = true; - - private string decodedSculptMapPath = null; private bool useMeshiesPhysicsMesh = false; private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh @@ -92,29 +88,9 @@ namespace OpenSim.Region.Physics.Meshing IConfig start_config = config.Configs["Startup"]; IConfig mesh_config = config.Configs["Mesh"]; - decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); - - cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps); - - if (Environment.OSVersion.Platform == PlatformID.Unix) - { - cacheSculptAlphaMaps = false; - } - else - cacheSculptAlphaMaps = cacheSculptMaps; - if(mesh_config != null) useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); - try - { - if (!Directory.Exists(decodedSculptMapPath)) - Directory.CreateDirectory(decodedSculptMapPath); - } - catch (Exception e) - { - m_log.WarnFormat("[SCULPT]: Unable to create {0} directory: ", decodedSculptMapPath, e.Message); - } } /// @@ -444,7 +420,7 @@ namespace OpenSim.Region.Physics.Meshing // physics_shape is an array of OSDMaps, one for each submesh if (decodedMeshOsd is OSDArray) { - // Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); +// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); decodedMeshOsdArray = (OSDArray)decodedMeshOsd; foreach (OSD subMeshOsd in decodedMeshOsdArray) @@ -717,29 +693,7 @@ namespace OpenSim.Region.Physics.Meshing faces = new List(); PrimMesher.SculptMesh sculptMesh; Image idata = null; - string decodedSculptFileName = ""; - if (cacheSculptMaps && primShape.SculptTexture != UUID.Zero) - { - decodedSculptFileName = System.IO.Path.Combine(decodedSculptMapPath, "smap_" + primShape.SculptTexture.ToString()); - try - { - if (File.Exists(decodedSculptFileName)) - { - idata = Image.FromFile(decodedSculptFileName); - } - } - catch (Exception e) - { - m_log.Error("[SCULPT]: unable to load cached sculpt map " + decodedSculptFileName + " " + e.Message); - - } - //if (idata != null) - // m_log.Debug("[SCULPT]: loaded cached map asset for map ID: " + primShape.SculptTexture.ToString()); - } - - if (idata == null) - { if (primShape.SculptData == null || primShape.SculptData.Length == 0) return false; @@ -748,25 +702,15 @@ namespace OpenSim.Region.Physics.Meshing OpenMetaverse.Imaging.ManagedImage unusedData; OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata); + unusedData = null; + if (idata == null) { // In some cases it seems that the decode can return a null bitmap without throwing // an exception m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap. Ignoring.", primName); - return false; } - - unusedData = null; - - //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData); - - if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0))) - // don't cache images with alpha channel in linux since mono can't load them correctly) - { - try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); } - catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); } - } } catch (DllNotFoundException) { @@ -783,7 +727,6 @@ namespace OpenSim.Region.Physics.Meshing m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message); return false; } - } PrimMesher.SculptMesh.SculptType sculptType; // remove mirror and invert bits @@ -1048,7 +991,6 @@ namespace OpenSim.Region.Physics.Meshing return ((hash << 5) + hash) + (ulong)(c >> 8); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) { return CreateMesh(primName, primShape, size, lod, false,false); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index c363310..f5bf05d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -172,7 +172,6 @@ namespace OpenSim.Region.Physics.OdePlugin // force lower density for testing m_density = 3.0f; - mu = parent_scene.AvatarFriction; walkDivisor = walk_divisor; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 702c336..3fcbb1b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -18,26 +18,63 @@ using OpenMetaverse; namespace OpenSim.Region.Physics.OdePlugin { + public enum meshWorkerCmnds : byte + { + nop = 0, + addnew, + changefull, + changesize, + changeshapetype, + getmesh, + } + + public class ODEPhysRepData + { + public PhysicsActor actor; + public PrimitiveBaseShape pbs; + public IMesh mesh; + + public Vector3 size; + public Vector3 OBB; + public Vector3 OBBOffset; + + public float volume; + + public float physCost; + public float streamCost; + public byte shapetype; + public bool hasOBB; + public bool hasMeshVolume; + public AssetState assetState; + public UUID? assetID; + public meshWorkerCmnds comand; + } + + + public class ODEMeshWorker { + private ILog m_log; private OdeScene m_scene; private IMesher m_mesher; - public bool meshSculptedPrim = true; public bool forceSimplePrimMeshing = false; public float meshSculptLOD = 32; public float MeshSculptphysicalLOD = 32; - private IntPtr m_workODEspace = IntPtr.Zero; - public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IntPtr pWorkSpace, IConfig pConfig) + private OpenSim.Framework.BlockingQueue createqueue = new OpenSim.Framework.BlockingQueue(); + private bool m_running; + + private Thread m_thread; + + public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig) { m_scene = pScene; m_log = pLog; m_mesher = pMesher; - m_workODEspace = pWorkSpace; if (pConfig != null) { @@ -46,8 +83,177 @@ namespace OpenSim.Region.Physics.OdePlugin meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD); MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); } + m_running = true; + m_thread = new Thread(DoWork); + m_thread.Start(); + } + + private void DoWork() + { + while(m_running) + { + ODEPhysRepData nextRep = createqueue.Dequeue(); + if(!m_running) + return; + if (nextRep == null) + continue; + if (m_scene.haveActor(nextRep.actor)) + { + switch (nextRep.comand) + { + case meshWorkerCmnds.changefull: + case meshWorkerCmnds.changeshapetype: + case meshWorkerCmnds.changesize: + if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor)) + m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep); + break; + case meshWorkerCmnds.addnew: + if (CreateActorPhysRep(nextRep)) + m_scene.AddChange(nextRep.actor, changes.AddPhysRep, nextRep); + break; + case meshWorkerCmnds.getmesh: + DoRepDataGetMesh(nextRep); + break; + } + } + } + } + + public void Stop() + { + m_running = false; + m_thread.Abort(); + } + + public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, + Vector3 size, byte shapetype) + { + ODEPhysRepData repData = new ODEPhysRepData(); + repData.actor = actor; + repData.pbs = pbs; + repData.size = size; + repData.shapetype = shapetype; + + // if (CheckMeshDone(repData)) + { + CheckMeshDone(repData); + CalcVolumeData(repData); + m_scene.AddChange(actor, changes.PhysRepData, repData); + return; + } + +// repData.comand = meshWorkerCmnds.changefull; +// createqueue.Enqueue(repData); + } + + public void NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, + Vector3 size, byte shapetype) + { + ODEPhysRepData repData = new ODEPhysRepData(); + repData.actor = actor; + repData.pbs = pbs; + repData.size = size; + repData.shapetype = shapetype; + + // bool done = CheckMeshDone(repData); + + CheckMeshDone(repData); + CalcVolumeData(repData); + m_scene.AddChange(actor, changes.AddPhysRep, repData); +// if (done) + return; + +// repData.comand = meshWorkerCmnds.addnew; +// createqueue.Enqueue(repData); + } + + public void RequestMeshAsset(ODEPhysRepData repData) + { + if (repData.assetState != AssetState.needAsset) + return; + + if (repData.assetID == null || repData.assetID == UUID.Zero) + return; + + repData.mesh = null; + + repData.assetState = AssetState.loadingAsset; + + repData.comand = meshWorkerCmnds.getmesh; + createqueue.Enqueue(repData); + } + + public bool CreateActorPhysRep(ODEPhysRepData repData) + { + getMesh(repData); + IMesh mesh = repData.mesh; + + if (mesh != null) + { + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}", + repData.actor.Name, repData.pbs.SculptTexture.ToString()); + repData.assetState = AssetState.AssetFailed; + repData.hasOBB = false; + repData.mesh = null; + m_scene.mesher.ReleaseMesh(mesh); + } + else + { + repData.OBBOffset = mesh.GetCentroid(); + repData.OBB = mesh.GetOBB(); + repData.hasOBB = true; + repData.physCost = 0.0013f * (float)indexCount; + // todo + repData.streamCost = 1.0f; + mesh.releaseSourceMeshData(); + } + } + CalcVolumeData(repData); + return true; + } + + public void AssetLoaded(ODEPhysRepData repData) + { + if (m_scene.haveActor(repData.actor)) + { + if (needsMeshing(repData.pbs)) // no need for pbs now? + { + repData.comand = meshWorkerCmnds.changefull; + createqueue.Enqueue(repData); + } + } } + public void DoRepDataGetMesh(ODEPhysRepData repData) + { + if (!repData.pbs.SculptEntry) + return; + + if (repData.assetState != AssetState.loadingAsset) + return; + + if (repData.assetID == null || repData.assetID == UUID.Zero) + return; + + if (repData.assetID != repData.pbs.SculptTexture) + return; + + RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; + if (assetProvider == null) + return; + ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log); + } + + /// /// Routine to figure out if we need to mesh this prim with our mesher /// @@ -169,194 +375,151 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } - public IMesh getMesh(PhysicsActor actor, PrimitiveBaseShape ppbs, Vector3 psize, byte pshapetype) + public bool CheckMeshDone(ODEPhysRepData repData) { - if (!(actor is OdePrim)) - return null; + PhysicsActor actor = repData.actor; + PrimitiveBaseShape pbs = repData.pbs; - IMesh mesh = null; - PrimitiveBaseShape pbs = ppbs; - Vector3 size = psize; - byte shapetype = pshapetype; + repData.mesh = null; + repData.hasOBB = false; - if (needsMeshing(pbs)) + if (!needsMeshing(pbs)) { - bool convex; - int clod = (int)LevelOfDetail.High; - if (shapetype == 0) - convex = false; - else - { - convex = true; - if (pbs.SculptType != (byte)SculptType.Mesh) - clod = (int)LevelOfDetail.Low; - } - mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); - if (mesh == null) - { - if (!pbs.SculptEntry) - return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); - - if (pbs.SculptTexture == UUID.Zero) - return null; - - if (pbs.SculptType != (byte)SculptType.Mesh) - { // check for sculpt decoded image on cache) - if (File.Exists(System.IO.Path.Combine("j2kDecodeCache", "smap_" + pbs.SculptTexture.ToString()))) - return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); - } - - if (pbs.SculptData != null && pbs.SculptData.Length > 0) - return m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); - - ODEAssetRequest asr; - RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; - if (assetProvider != null) - asr = new ODEAssetRequest(this, assetProvider, actor, pbs, m_log); + repData.assetState = AssetState.noNeedAsset; + return true; + } - return null; + if (pbs.SculptEntry) + { + if (repData.assetState == AssetState.AssetFailed) + { + if (pbs.SculptTexture == repData.assetID) + return true; } } - return mesh; - } + else + { + repData.assetState = AssetState.noNeedAsset; + repData.assetID = null; + } - private bool GetTriMeshGeo(ODEPhysRepData repData) - { - IntPtr vertices, indices; - IntPtr triMeshData = IntPtr.Zero; - IntPtr geo = IntPtr.Zero; - int vertexCount, indexCount; - int vertexStride, triStride; + IMesh mesh = null; - PhysicsActor actor = repData.actor; + Vector3 size = repData.size; + byte shapetype = repData.shapetype; - IMesh mesh = repData.mesh; + bool convex; - if (mesh == null) + int clod = (int)LevelOfDetail.High; + if (shapetype == 0) + convex = false; + else { - mesh = getMesh(repData.actor, repData.pbs, repData.size, repData.shapetype); + convex = true; + if (pbs.SculptType != (byte)SculptType.Mesh) + clod = (int)LevelOfDetail.Low; } - + mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); if (mesh == null) + { + if (pbs.SculptEntry) + { + if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero) + { + repData.assetID = pbs.SculptTexture; + repData.assetState = AssetState.needAsset; + } + else + repData.assetState = AssetState.AssetFailed; + } return false; + } - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage - - if (vertexCount == 0 || indexCount == 0) + repData.mesh = mesh; + if (pbs.SculptEntry) { - m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}", - actor.Name, repData.pbs.SculptTexture.ToString()); - mesh.releaseSourceMeshData(); - return false; + repData.assetState = AssetState.AssetOK; + repData.assetID = pbs.SculptTexture; + pbs.SculptData = Utils.EmptyBytes; } + return true; + } - repData.OBBOffset = mesh.GetCentroid(); - repData.OBB = mesh.GetOBB(); - repData.hasOBB = true; - repData.physCost = 0.0013f * (float)indexCount; - mesh.releaseSourceMeshData(); + public bool getMesh(ODEPhysRepData repData) + { + PhysicsActor actor = repData.actor; - try - { - triMeshData = d.GeomTriMeshDataCreate(); + PrimitiveBaseShape pbs = repData.pbs; - d.GeomTriMeshDataBuildSimple(triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); - d.GeomTriMeshDataPreprocess(triMeshData); + repData.mesh = null; + repData.hasOBB = false; - m_scene.waitForSpaceUnlock(m_workODEspace); - geo = d.CreateTriMesh(m_workODEspace, triMeshData, null, null, null); - } + if (!needsMeshing(pbs)) + return false; - catch (Exception e) + if (pbs.SculptEntry) { - m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", actor.Name, e); - if (triMeshData != IntPtr.Zero) + if (repData.assetState == AssetState.AssetFailed) { - d.GeomTriMeshDataDestroy(triMeshData); - repData.triMeshData = IntPtr.Zero; + if (pbs.SculptTexture == repData.assetID) + return true; } - repData.geo = IntPtr.Zero; - return false; } - repData.geo = geo; - repData.triMeshData = triMeshData; - repData.curSpace = m_workODEspace; - return true; - } - - public ODEPhysRepData CreateActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, IMesh pMesh, Vector3 size, byte shapetype) - { - ODEPhysRepData repData = new ODEPhysRepData(); + repData.assetState = AssetState.noNeedAsset; - repData.actor = actor; - repData.pbs = pbs; - repData.mesh = pMesh; - repData.size = size; - repData.shapetype = shapetype; + IMesh mesh = null; + Vector3 size = repData.size; + byte shapetype = repData.shapetype; - IntPtr geo = IntPtr.Zero; - bool hasMesh = false; - if (needsMeshing(pbs)) + bool convex; + int clod = (int)LevelOfDetail.High; + if (shapetype == 0) + convex = false; + else { - if (GetTriMeshGeo(repData)) - hasMesh = true; - else - repData.NoColide = true; + convex = true; + if (pbs.SculptType != (byte)SculptType.Mesh) + clod = (int)LevelOfDetail.Low; } - - if (!hasMesh) + mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); + if (mesh == null) { - if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 - && size.X == size.Y && size.Y == size.Z) - { // it's a sphere - m_scene.waitForSpaceUnlock(m_workODEspace); - try - { - geo = d.CreateSphere(m_workODEspace, size.X * 0.5f); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); - return null; - } - } - else - {// do it as a box - m_scene.waitForSpaceUnlock(m_workODEspace); - try - { - //Console.WriteLine(" CreateGeom 4"); - geo = d.CreateBox(m_workODEspace, size.X, size.Y, size.Z); - } - catch (Exception e) + if (pbs.SculptEntry) + { + if (pbs.SculptTexture == UUID.Zero) + return false; + + repData.assetID = pbs.SculptTexture; + repData.assetState = AssetState.AssetOK; + + if (pbs.SculptData == null || pbs.SculptData.Length == 0) { - m_log.Warn("[PHYSICS]: Create box failed: {0}", e); - return null; + repData.assetState = AssetState.needAsset; + return false; } } - repData.physCost = 0.1f; - repData.streamCost = 1.0f; - repData.geo = geo; + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + } - - repData.curSpace = m_workODEspace; - CalcVolumeData(repData); + repData.mesh = mesh; + repData.pbs.SculptData = Utils.EmptyBytes; - return repData; - } + if (mesh == null) + { + if (pbs.SculptEntry) + repData.assetState = AssetState.AssetFailed; - public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, - Vector3 size, byte shapetype, MeshWorkerChange what) - { - ODEPhysRepData repData = CreateActorPhysRep(actor, pbs, null, size, shapetype); - repData.changed |= what; - if (repData != null && actor != null) - ((OdePrim)actor).AddChange(changes.PhysRepData, repData); + return false; + } + + if (pbs.SculptEntry) + repData.assetState = AssetState.AssetOK; + + return true; } private void CalculateBasicPrimVolume(ODEPhysRepData repData) @@ -662,46 +825,12 @@ namespace OpenSim.Region.Physics.OdePlugin private void CalcVolumeData(ODEPhysRepData repData) { - float volume; - Vector3 OBB = repData.size; - Vector3 OBBoffset; - IntPtr geo = repData.geo; - - if (geo == IntPtr.Zero || repData.triMeshData == IntPtr.Zero) - { - OBB.X *= 0.5f; - OBB.Y *= 0.5f; - OBB.Z *= 0.5f; - - repData.OBB = OBB; - repData.OBBOffset = Vector3.Zero; - } - else if (!repData.hasOBB) // should this happen? - { - d.AABB AABB; - d.GeomGetAABB(geo, out AABB); // get the AABB from engine geom - - OBB.X = (AABB.MaxX - AABB.MinX) * 0.5f; - OBB.Y = (AABB.MaxY - AABB.MinY) * 0.5f; - OBB.Z = (AABB.MaxZ - AABB.MinZ) * 0.5f; - repData.OBB = OBB; - OBBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; - OBBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; - OBBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; - repData.OBBOffset = Vector3.Zero; - } - - // also its own inertia and mass - // keep using basic shape mass for now - CalculateBasicPrimVolume(repData); - if (repData.hasOBB) { - OBB = repData.OBB; + Vector3 OBB = repData.OBB; float pc = repData.physCost; float psf = OBB.X * (OBB.Y + OBB.Z) + OBB.Y * OBB.Z; psf *= 1.33f * .2f; - pc *= psf; if (pc < 0.1f) pc = 0.1f; @@ -709,54 +838,79 @@ namespace OpenSim.Region.Physics.OdePlugin repData.physCost = pc; } else + { + Vector3 OBB = repData.size; + OBB.X *= 0.5f; + OBB.Y *= 0.5f; + OBB.Z *= 0.5f; + + repData.OBB = OBB; + repData.OBBOffset = Vector3.Zero; + repData.physCost = 0.1f; + repData.streamCost = 1.0f; + } + + CalculateBasicPrimVolume(repData); } } public class ODEAssetRequest { - PhysicsActor m_actor; ODEMeshWorker m_worker; - PrimitiveBaseShape m_pbs; private ILog m_log; + ODEPhysRepData repData; public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider, - PhysicsActor pActor, PrimitiveBaseShape ppbs, ILog plog) + ODEPhysRepData pRepData, ILog plog) { - m_actor = pActor; m_worker = pWorker; - m_pbs = ppbs; m_log = plog; + repData = pRepData; + repData.assetState = AssetState.AssetFailed; if (provider == null) return; - UUID assetID = m_pbs.SculptTexture; + if (repData.assetID == null) + return; + + UUID assetID = (UUID) repData.assetID; if (assetID == UUID.Zero) return; + repData.assetState = AssetState.loadingAsset; provider(assetID, ODEassetReceived); } void ODEassetReceived(AssetBase asset) { - if (m_actor != null && m_pbs != null) + repData.assetState = AssetState.AssetFailed; + if (asset != null) { - if (asset != null) + if (asset.Data != null && asset.Data.Length > 0) { - if (asset.Data != null && asset.Data.Length > 0) - { - m_pbs.SculptData = asset.Data; - m_actor.Shape = m_pbs; - } - else - m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.", - m_actor.Name, asset.ID.ToString()); + if (!repData.pbs.SculptEntry) + return; + if (repData.pbs.SculptTexture != repData.assetID) + return; + + // asset get may return a pointer to the same asset data + // for similar prims and we destroy with it + // so waste a lot of time stressing gc and hoping it clears things + // TODO avoid this + repData.pbs.SculptData = new byte[asset.Data.Length]; + asset.Data.CopyTo(repData.pbs.SculptData,0); + repData.assetState = AssetState.AssetOK; + m_worker.AssetLoaded(repData); } else - m_log.WarnFormat("[PHYSICS]: asset provider returned null asset fo mesh of prim {0}.", - m_actor.Name); + m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.", + repData.actor.Name, asset.ID.ToString()); } + else + m_log.WarnFormat("[PHYSICS]: asset provider returned null asset fo mesh of prim {0}.", + repData.actor.Name); } } } \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 7650571..cbe129a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -79,7 +79,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_lastdoneSelected; internal bool m_outbounds; - private Quaternion m_lastorientation = new Quaternion(); + private Quaternion m_lastorientation; private Quaternion _orientation; private Vector3 _position; @@ -91,14 +91,14 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 _size; private Vector3 _acceleration; private Vector3 m_angularlock = Vector3.One; - private IntPtr Amotor = IntPtr.Zero; + private IntPtr Amotor; private Vector3 m_force; private Vector3 m_forceacc; private Vector3 m_angularForceacc; - private float m_invTimeStep = 50.0f; - private float m_timeStep = .02f; + private float m_invTimeStep; + private float m_timeStep; private Vector3 m_PIDTarget; private float m_PIDTau; @@ -107,14 +107,14 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_PIDHoverHeight; private float m_PIDHoverTau; private bool m_useHoverPID; - private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private PIDHoverType m_PIDHoverType; private float m_targetHoverHeight; private float m_groundHeight; private float m_waterHeight; private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - private int body_autodisable_frames = 5; - public int bodydisablecontrol = 0; + private int body_autodisable_frames; + public int bodydisablecontrol; // Default we're a Geometry @@ -144,12 +144,16 @@ namespace OpenSim.Region.Physics.OdePlugin private IMesh m_mesh; private object m_meshlock = new object(); private PrimitiveBaseShape _pbs; + + private UUID? m_assetID; + private AssetState m_assetState; + public OdeScene _parent_scene; /// /// The physics space which contains prim geometry /// - public IntPtr m_targetSpace = IntPtr.Zero; + public IntPtr m_targetSpace; public IntPtr prim_geom; public IntPtr _triMeshData; @@ -163,27 +167,32 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr collide_geom; // for objects: geom if single prim space it linkset - private float m_density = 10.000006836f; // Aluminum g/cm3; + private float m_density; private byte m_shapetype; public bool _zeroFlag; private bool m_lastUpdateSent; - public IntPtr Body = IntPtr.Zero; + public IntPtr Body; private Vector3 _target_velocity; - public Vector3 primOOBsize; // prim real dimensions from mesh - public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb + public Vector3 m_OBBOffset; + public Vector3 m_OBB; public float primOOBradiusSQ; + + private bool m_hasOBB = true; + + private float m_physCost; + private float m_streamCost; + public d.Mass primdMass; // prim inertia information on it's own referencial float primMass; // prim own mass float primVolume; // prim own volume; float _mass; // object mass acording to case - private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb - public int givefakepos = 0; + public int givefakepos; private Vector3 fakepos; - public int givefakeori = 0; + public int givefakeori; private Quaternion fakeori; private int m_eventsubscription; @@ -391,9 +400,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (value.IsFinite()) { - AddChange(changes.Size, value); - -// _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype, MeshWorkerChange.size); + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype); } else { @@ -463,7 +470,7 @@ namespace OpenSim.Region.Physics.OdePlugin q.Z = dq.Z; q.W = dq.W; - Vector3 Ptot = primOOBoffset * q; + Vector3 Ptot = m_OBBOffset * q; dtmp = d.GeomGetPosition(prim_geom); Ptot.X += dtmp.X; Ptot.Y += dtmp.Y; @@ -503,7 +510,7 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - return primOOBsize; + return m_OBB; } } @@ -511,7 +518,7 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - return primOOBoffset; + return m_OBBOffset; } } @@ -527,8 +534,8 @@ namespace OpenSim.Region.Physics.OdePlugin { set { - AddChange(changes.Shape, value); -// _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype, MeshWorkerChange.shape); +// AddChange(changes.Shape, value); + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype); } } @@ -541,8 +548,7 @@ namespace OpenSim.Region.Physics.OdePlugin set { m_shapetype = value; - AddChange(changes.Shape, null); -// _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value, MeshWorkerChange.shapetype); + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); } } @@ -1012,7 +1018,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_invTimeStep = 1f / m_timeStep; m_density = parent_scene.geomDefaultDensity; - // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; body_autodisable_frames = parent_scene.bodyFramesAutoDisable; prim_geom = IntPtr.Zero; @@ -1064,7 +1069,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_colliderfilter = 0; m_NoColide = false; - hasOOBoffsetFromMesh = false; _triMeshData = IntPtr.Zero; m_shapetype = _shapeType; @@ -1079,29 +1083,9 @@ namespace OpenSim.Region.Physics.OdePlugin mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; - CalcPrimBodyData(); -/* - m_mesh = null; - if (_parent_scene.needsMeshing(pbs) && (pbs.SculptData.Length > 0)) - { - bool convex; - int clod = (int)LevelOfDetail.High; - if (m_shapetype == 0) - convex = false; - else - { - convex = true; - if (_pbs.SculptType != (byte)SculptType.Mesh) - clod = (int)LevelOfDetail.Low; - } - m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, clod, true, convex); - } -*/ - m_mesh = _parent_scene.m_meshWorker.getMesh(this, pbs, _size, m_shapetype); - m_building = true; // control must set this to false when done - AddChange(changes.Add, null); + _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); } private void resetCollisionAccounting() @@ -1325,83 +1309,34 @@ namespace OpenSim.Region.Physics.OdePlugin } } - private bool setMesh(OdeScene parent_scene) + private bool GetMeshGeom() { IntPtr vertices, indices; int vertexCount, indexCount; int vertexStride, triStride; - if (Body != IntPtr.Zero) - { - if (childPrim) - { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this, false); - } - } - else - { - DestroyBody(); - } - } - - IMesh mesh = null; - - lock (m_meshlock) - { - if (m_mesh == null) - { -/* - bool convex; - int clod = (int)LevelOfDetail.High; - - if (m_shapetype == 0) - convex = false; - else - { - convex = true; - if (_pbs.SculptType != (byte)SculptType.Mesh) - clod = (int)LevelOfDetail.Low; - } - - mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, clod, true, convex); -*/ - mesh = _parent_scene.m_meshWorker.getMesh(this, _pbs, _size, m_shapetype); - } - else - { - mesh = m_mesh; - } - - if (mesh == null) - { - m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); - return false; - } - + IMesh mesh = m_mesh; - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage - - if (vertexCount == 0 || indexCount == 0) - { - m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", - Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); - mesh.releaseSourceMeshData(); - return false; - } - - primOOBoffset = mesh.GetCentroid(); - hasOOBoffsetFromMesh = true; + if (mesh == null) + return false; - mesh.releaseSourceMeshData(); - m_mesh = mesh; + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0} mesh UUID {1}", + Name, _pbs.SculptTexture.ToString()); + m_hasOBB = false; + m_OBBOffset = Vector3.Zero; + m_OBB = _size * 0.5f; + m_physCost = 0.1f; + m_streamCost = 1.0f; + _parent_scene.mesher.ReleaseMesh(mesh); + m_assetState = AssetState.AssetFailed; + m_mesh = null; + return false; } - - IntPtr geo = IntPtr.Zero; - try { _triMeshData = d.GeomTriMeshDataCreate(); @@ -1409,8 +1344,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); d.GeomTriMeshDataPreprocess(_triMeshData); - _parent_scene.waitForSpaceUnlock(m_targetSpace); - geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + prim_geom = d.CreateTriMesh(IntPtr.Zero, _triMeshData, null, null, null); } catch (Exception e) @@ -1418,85 +1352,56 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); if (_triMeshData != IntPtr.Zero) { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - return false; - } - - SetGeom(geo); - return true; - } - - private void SetGeom(IntPtr geom) - { - prim_geom = geom; - //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); - if (prim_geom != IntPtr.Zero) - { - - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) + try { - d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); + d.GeomTriMeshDataDestroy(_triMeshData); } - else + catch { - d.GeomSetCollideBits(prim_geom, 0); - d.GeomDisable(prim_geom); } } - else - { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); - } - - CalcPrimBodyData(); - - _parent_scene.actor_name_map[prim_geom] = this; + _triMeshData = IntPtr.Zero; + prim_geom = IntPtr.Zero; + m_hasOBB = false; + m_OBBOffset = Vector3.Zero; + m_OBB = _size * 0.5f; + m_physCost = 0.1f; + m_streamCost = 1.0f; + _parent_scene.mesher.ReleaseMesh(mesh); + m_assetState = AssetState.AssetFailed; + m_mesh = null; + return false; } - else - m_log.Warn("Setting bad Geom"); + return true; } - - /// - /// Create a geometry for the given mesh in the given target space. - /// - /// - /// If null, then a mesh is used that is based on the profile shape data. private void CreateGeom() { - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } + IntPtr geo = IntPtr.Zero; + bool hasMesh = false; - bool haveMesh = false; - hasOOBoffsetFromMesh = false; m_NoColide = false; - if (_parent_scene.m_meshWorker.needsMeshing(_pbs)) + if (m_assetState == AssetState.AssetFailed) + m_NoColide = true; + + else if (m_mesh != null) { - haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims - if (!haveMesh) + if (GetMeshGeom()) + hasMesh = true; + else m_NoColide = true; } - if (!haveMesh) + if (!hasMesh) { if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 && _size.X == _size.Y && _size.Y == _size.Z) { // it's a sphere - _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); + geo = d.CreateSphere(IntPtr.Zero, _size.X * 0.5f); } catch (Exception e) { @@ -1506,11 +1411,9 @@ namespace OpenSim.Region.Physics.OdePlugin } else {// do it as a box - _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - //Console.WriteLine(" CreateGeom 4"); - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + geo = d.CreateBox(IntPtr.Zero, _size.X, _size.Y, _size.Z); } catch (Exception e) { @@ -1518,18 +1421,18 @@ namespace OpenSim.Region.Physics.OdePlugin return; } } + m_physCost = 0.1f; + m_streamCost = 1.0f; + prim_geom = geo; } } - /// - /// Set a new geometry for this prim. - /// - /// private void RemoveGeom() { if (prim_geom != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(prim_geom); + try { d.GeomDestroy(prim_geom); @@ -1546,6 +1449,7 @@ namespace OpenSim.Region.Physics.OdePlugin prim_geom = IntPtr.Zero; collide_geom = IntPtr.Zero; + m_targetSpace = IntPtr.Zero; } else { @@ -1562,7 +1466,7 @@ namespace OpenSim.Region.Physics.OdePlugin } Body = IntPtr.Zero; - hasOOBoffsetFromMesh = false; + m_hasOBB = false; } //sets non physical prim m_targetSpace to right space in spaces grid for static prims @@ -2136,358 +2040,6 @@ namespace OpenSim.Region.Physics.OdePlugin #region Mass Calculation - private float CalculatePrimVolume() - { - float volume = _size.X * _size.Y * _size.Z; // default - float tmp; - - float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - - switch (_pbs.ProfileShape) - { - case ProfileShape.Square: - // default box - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - - hollowVolume *= (0.5f * .5f); - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - //a tube - - volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume * tmp * tmp; - - if (hollowAmount > 0.0) - { - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - hollowVolume *= 0.5f * 0.5f; - break; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - break; - - case ProfileShape.Circle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.78539816339f; // elipse base - - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - case ProfileShape.HalfCircle: - if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.5236f; - - if (hollowAmount > 0.0) - { - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Circle: - case HollowShape.Triangle: // diference in sl is minor and odd - case HollowShape.Same: - break; - - case HollowShape.Square: - hollowVolume *= 0.909f; - break; - - // case HollowShape.Triangle: - // hollowVolume *= .827f; - // break; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - - } - break; - - case ProfileShape.EquilateralTriangle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.32475953f; - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.32475953f; - volume *= 0.01f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - default: - break; - } - - float taperX1; - float taperY1; - float taperX; - float taperY; - float pathBegin; - float pathEnd; - float profileBegin; - float profileEnd; - - if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) - { - taperX1 = _pbs.PathScaleX * 0.01f; - if (taperX1 > 1.0f) - taperX1 = 2.0f - taperX1; - taperX = 1.0f - taperX1; - - taperY1 = _pbs.PathScaleY * 0.01f; - if (taperY1 > 1.0f) - taperY1 = 2.0f - taperY1; - taperY = 1.0f - taperY1; - } - else - { - taperX = _pbs.PathTaperX * 0.01f; - if (taperX < 0.0f) - taperX = -taperX; - taperX1 = 1.0f - taperX; - - taperY = _pbs.PathTaperY * 0.01f; - if (taperY < 0.0f) - taperY = -taperY; - taperY1 = 1.0f - taperY; - } - - volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); - - pathBegin = (float)_pbs.PathBegin * 2.0e-5f; - pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; - volume *= (pathEnd - pathBegin); - - // this is crude aproximation - profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; - profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; - volume *= (profileEnd - profileBegin); - - return volume; - } - - - private void CalcPrimBodyData() - { - float volume; - - if (prim_geom == IntPtr.Zero) - { - // Ubit let's have a initial basic OOB - primOOBsize.X = _size.X; - primOOBsize.Y = _size.Y; - primOOBsize.Z = _size.Z; - primOOBoffset = Vector3.Zero; - } - else - { - d.AABB AABB; - d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom - - primOOBsize.X = (AABB.MaxX - AABB.MinX); - primOOBsize.Y = (AABB.MaxY - AABB.MinY); - primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); - if (!hasOOBoffsetFromMesh) - { - primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; - primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; - primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; - } - } - - // also its own inertia and mass - // keep using basic shape mass for now - volume = CalculatePrimVolume(); - - primVolume = volume; - primMass = m_density * volume; - - if (primMass <= 0) - primMass = 0.0001f;//ckrinke: Mass must be greater then zero. - if (primMass > _parent_scene.maximumMassObject) - primMass = _parent_scene.maximumMassObject; - - _mass = primMass; // just in case - - d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); - - d.MassTranslate(ref primdMass, - primOOBoffset.X, - primOOBoffset.Y, - primOOBoffset.Z); - - primOOBsize *= 0.5f; // let obb size be a corner coords - primOOBradiusSQ = primOOBsize.LengthSquared(); - } - private void UpdatePrimBodyData() { primMass = m_density * primVolume; @@ -2499,14 +2051,14 @@ namespace OpenSim.Region.Physics.OdePlugin _mass = primMass; // just in case - d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); + d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z); d.MassTranslate(ref primdMass, - primOOBoffset.X, - primOOBoffset.Y, - primOOBoffset.Z); + m_OBBOffset.X, + m_OBBOffset.Y, + m_OBBOffset.Z); - primOOBradiusSQ = primOOBsize.LengthSquared(); + primOOBradiusSQ = m_OBBOffset.LengthSquared(); } #endregion @@ -2697,27 +2249,6 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeadd() { - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - - if (!m_isphysical) - { - SetInStaticSpace(this); - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - else - MakeBody(); - } } private void changeAngularLock(Vector3 newLock) @@ -3161,41 +2692,45 @@ namespace OpenSim.Region.Physics.OdePlugin resetCollisionAccounting(); } - private void changeprimsizeshape() + private void changeSize(Vector3 newSize) { - CheckDelaySelect(); + } - OdePrim parent = (OdePrim)_parent; + private void changeShape(PrimitiveBaseShape newShape) + { + } - bool chp = childPrim; + - if (chp) - { - if (parent != null) - { - parent.DestroyBody(); - } - } - else - { - DestroyBody(); - } + private void changeAddPhysRep(ODEPhysRepData repData) + { + _size = repData.size; //?? + _pbs = repData.pbs; + m_shapetype = repData.shapetype; - RemoveGeom(); + m_mesh = repData.mesh; - // we don't need to do space calculation because the client sends a position update also. - if (_size.X <= 0) - _size.X = 0.01f; - if (_size.Y <= 0) - _size.Y = 0.01f; - if (_size.Z <= 0) - _size.Z = 0.01f; - // Construction of new prim + m_assetID = repData.assetID; + m_assetState = repData.assetState; + + m_hasOBB = repData.hasOBB; + m_OBBOffset = repData.OBBOffset; + m_OBB = repData.OBB; + +// m_NoColide = repData.NoColide; + m_physCost = repData.physCost; + m_streamCost = repData.streamCost; + + primVolume = repData.volume; CreateGeom(); if (prim_geom != IntPtr.Zero) { + UpdatePrimBodyData(); + + _parent_scene.actor_name_map[prim_geom] = this; + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); d.Quaternion myrot = new d.Quaternion(); myrot.X = _orientation.X; @@ -3203,44 +2738,26 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.Z = _orientation.Z; myrot.W = _orientation.W; d.GeomSetQuaternion(prim_geom, ref myrot); - } - if (m_isphysical) - { - if (chp) + if (!m_isphysical) { - if (parent != null) - { - parent.MakeBody(); - } + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); } else - MakeBody(); - } + MakeBody(); - else - { - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); + if (m_assetState == AssetState.needAsset) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_shapetype; + _parent_scene.m_meshWorker.RequestMeshAsset(repData); + } } - - resetCollisionAccounting(); - } - - private void changeSize(Vector3 newSize) - { - _size = newSize; - changeprimsizeshape(); } - private void changeShape(PrimitiveBaseShape newShape) - { - if(newShape != null) - _pbs = newShape; - changeprimsizeshape(); - } - - private void changePhysRepData(ODEPhysRepData repData) { CheckDelaySelect(); @@ -3261,32 +2778,43 @@ namespace OpenSim.Region.Physics.OdePlugin DestroyBody(); } - RemoveGeom(); + RemoveGeom(); - prim_geom = repData.geo; - _triMeshData = repData.triMeshData; _size = repData.size; _pbs = repData.pbs; - m_mesh = repData.mesh; m_shapetype = repData.shapetype; - hasOOBoffsetFromMesh = repData.hasOBB; - primOOBoffset = repData.OBBOffset; - primOOBsize = repData.OBB; + m_mesh = repData.mesh; + + m_assetID = repData.assetID; + m_assetState = repData.assetState; - m_NoColide = repData.NoColide; -// m_physCost = repData.physCost; -// m_streamCost = repData.streamCost; + m_hasOBB = repData.hasOBB; + m_OBBOffset = repData.OBBOffset; + m_OBB = repData.OBB; - primVolume = repData.volume; - m_targetSpace = repData.curSpace; + m_physCost = repData.physCost; + m_streamCost = repData.streamCost; - UpdatePrimBodyData(); + primVolume = repData.volume; - _parent_scene.actor_name_map[prim_geom] = this; + CreateGeom(); if (prim_geom != IntPtr.Zero) { + m_targetSpace = IntPtr.Zero; + + UpdatePrimBodyData(); + + try + { + _parent_scene.actor_name_map[prim_geom] = this; + } + catch + { + + } + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); d.Quaternion myrot = new d.Quaternion(); myrot.X = _orientation.X; @@ -3294,32 +2822,39 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.Z = _orientation.Z; myrot.W = _orientation.W; d.GeomSetQuaternion(prim_geom, ref myrot); - } - if (m_isphysical) - { - if (chp) + + if (m_isphysical) { - if (parent != null) + if (chp) { - parent.MakeBody(); + if (parent != null) + { + parent.MakeBody(); + } } + else + MakeBody(); } + else - MakeBody(); - } + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } - else - { - SetInStaticSpace(this); - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); + resetCollisionAccounting(); + if (m_assetState == AssetState.needAsset) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_shapetype; + _parent_scene.m_meshWorker.RequestMeshAsset(repData); + } } - - resetCollisionAccounting(); } - private void changeFloatOnWater(bool newval) { m_collidesWater = newval; @@ -3984,7 +3519,7 @@ namespace OpenSim.Region.Physics.OdePlugin public bool DoAChange(changes what, object arg) { - if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) { return false; } @@ -3995,6 +3530,11 @@ namespace OpenSim.Region.Physics.OdePlugin case changes.Add: changeadd(); break; + + case changes.AddPhysRep: + changeAddPhysRep((ODEPhysRepData)arg); + break; + case changes.Remove: //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... //When we return true, it destroys all of the prims in the linkset anyway diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index d758c85..5e4c2a5 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -60,29 +60,6 @@ namespace OpenSim.Region.Physics.OdePlugin public int lastframe; } - public class ODEPhysRepData - { - public PhysicsActor actor; - public IntPtr geo = IntPtr.Zero; - public IntPtr triMeshData = IntPtr.Zero; - public IMesh mesh; - public IntPtr curSpace = IntPtr.Zero; - public PrimitiveBaseShape pbs; - - public Vector3 size = Vector3.Zero; - public Vector3 OBB = Vector3.Zero; - public Vector3 OBBOffset = Vector3.Zero; - - public float volume; - - public float physCost = 0.0f; - public float streamCost = 0; - public MeshWorkerChange changed; - public byte shapetype = 0; - public bool NoColide = false; - public bool hasOBB = false; - public bool hasMeshVolume = false; - } // colision flags of things others can colide with // rays, sensors, probes removed since can't be colided with @@ -133,13 +110,16 @@ namespace OpenSim.Region.Physics.OdePlugin light = 7 // compatibility with old viewers } - [Flags] - public enum MeshWorkerChange : uint + + public enum AssetState : byte { - none = 0, - size = 1, - shape = 2, - shapetype = 3, + noNeedAsset = 0, + needAsset = 1, + loadingAsset = 2, + procAsset = 3, + AssetOK = 4, + + AssetFailed = 0xff } public enum changes : int @@ -180,6 +160,7 @@ namespace OpenSim.Region.Physics.OdePlugin Size, Shape, PhysRepData, + AddPhysRep, CollidesWater, VolumeDtc, @@ -330,7 +311,6 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr TopSpace; // the global space public IntPtr ActiveSpace; // space for active prims public IntPtr StaticSpace; // space for the static things around - public IntPtr WorkSpace; // no collisions work space // some speedup variables private int spaceGridMaxX; @@ -342,7 +322,7 @@ namespace OpenSim.Region.Physics.OdePlugin private IntPtr[] staticPrimspaceOffRegion; public Object OdeLock; - private static Object SimulationLock; + public static Object SimulationLock; public IMesher mesher; @@ -403,7 +383,6 @@ namespace OpenSim.Region.Physics.OdePlugin // now the major subspaces ActiveSpace = d.HashSpaceCreate(TopSpace); StaticSpace = d.HashSpaceCreate(TopSpace); - WorkSpace = d.HashSpaceCreate(TopSpace); } catch { @@ -413,12 +392,10 @@ namespace OpenSim.Region.Physics.OdePlugin d.HashSpaceSetLevels(TopSpace, -2, 8); d.HashSpaceSetLevels(ActiveSpace, -2, 8); d.HashSpaceSetLevels(StaticSpace, -2, 8); - d.HashSpaceSetLevels(WorkSpace, -2, 8); // demote to second level d.SpaceSetSublevel(ActiveSpace, 1); d.SpaceSetSublevel(StaticSpace, 1); - d.SpaceSetSublevel(WorkSpace, 1); d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | CollisionCategories.Geom | @@ -436,8 +413,6 @@ namespace OpenSim.Region.Physics.OdePlugin )); d.GeomSetCollideBits(StaticSpace, 0); - d.GeomSetCategoryBits(WorkSpace, 0); - d.GeomSetCollideBits(WorkSpace, 0); contactgroup = d.JointGroupCreate(0); //contactgroup @@ -518,7 +493,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, WorkSpace, physicsconfig); + m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); HalfOdeStep = ODE_STEPSIZE * 0.5f; odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); @@ -1316,6 +1291,15 @@ namespace OpenSim.Region.Physics.OdePlugin _collisionEventPrimRemove.Add(obj); } + public override float TimeDilation + { + get { return m_timeDilation; } + } + + public override bool SupportsNINJAJoints + { + get { return false; } + } #region Add/Remove Entities @@ -1371,117 +1355,59 @@ namespace OpenSim.Region.Physics.OdePlugin ((OdeCharacter) actor).Destroy(); } - private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - PrimitiveBaseShape pbs, bool isphysical, uint localID) - { - Vector3 pos = position; - Vector3 siz = size; - Quaternion rot = rotation; - OdePrim newPrim; - lock (OdeLock) + public void addActivePrim(OdePrim activatePrim) + { + // adds active prim.. + lock (_activeprims) { - newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,0,localID); - - lock (_prims) - _prims.Add(newPrim); + if (!_activeprims.Contains(activatePrim)) + _activeprims.Add(activatePrim); } - return newPrim; } - private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, uint localID) + public void addActiveGroups(OdePrim activatePrim) { - Vector3 pos = position; - Vector3 siz = size; - Quaternion rot = rotation; - - OdePrim newPrim; - lock (OdeLock) + lock (_activegroups) { - newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, 0, localID); - - lock (_prims) - _prims.Add(newPrim); + if (!_activegroups.Contains(activatePrim)) + _activegroups.Add(activatePrim); } - return newPrim; } private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) { - Vector3 pos = position; - Vector3 siz = size; - Quaternion rot = rotation; - OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, shapeType, localID); - + newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); lock (_prims) _prims.Add(newPrim); } return newPrim; } - public void addActivePrim(OdePrim activatePrim) - { - // adds active prim.. - lock (_activeprims) - { - if (!_activeprims.Contains(activatePrim)) - _activeprims.Add(activatePrim); - } - } - - public void addActiveGroups(OdePrim activatePrim) - { - lock (_activegroups) - { - if (!_activegroups.Contains(activatePrim)) - _activegroups.Add(activatePrim); - } - } - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) { - return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, localid); + return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { -#if SPAM - m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); -#endif - - return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid); + return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) { -#if SPAM - m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); -#endif return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); } - public override float TimeDilation - { - get { return m_timeDilation; } - } - - public override bool SupportsNINJAJoints - { - get { return false; } - } - - public void remActivePrim(OdePrim deactivatePrim) { lock (_activeprims) @@ -1534,6 +1460,28 @@ namespace OpenSim.Region.Physics.OdePlugin } } + + public bool havePrim(OdePrim prm) + { + lock (_prims) + return _prims.Contains(prm); + } + + public bool haveActor(PhysicsActor actor) + { + if (actor is OdePrim) + { + lock (_prims) + return _prims.Contains((OdePrim)actor); + } + else if (actor is OdeCharacter) + { + lock (_characters) + return _characters.Contains((OdeCharacter)actor); + } + return false; + } + #endregion #region Space Separation Calculation @@ -1706,20 +1654,9 @@ namespace OpenSim.Region.Physics.OdePlugin { if (world == IntPtr.Zero) return 0; + + d.WorldSetQuickStepNumIterations(world, curphysiteractions); - // adjust number of iterations per step - -// try -// { - d.WorldSetQuickStepNumIterations(world, curphysiteractions); -/* } - catch (StackOverflowException) - { - m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); -// ode.drelease(world); - base.TriggerPhysicsBasedRestart(); - } -*/ while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever { try @@ -1747,8 +1684,9 @@ namespace OpenSim.Region.Physics.OdePlugin } catch { - m_log.Warn("[PHYSICS]: doChange failed for a actor"); - }; + m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); + } } ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); if (ttmp > 20) @@ -2491,6 +2429,9 @@ namespace OpenSim.Region.Physics.OdePlugin */ public override void Dispose() { + if (m_meshWorker != null) + m_meshWorker.Stop(); + lock (OdeLock) { m_rayCastManager.Dispose(); -- cgit v1.1 From 4efc90ef37a244ae859f4789d1abbc332723a6ad Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 7 Oct 2012 05:53:52 +0100 Subject: i update core ode plugin and make it load is meshs (i hope) --- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 90 +++++++++++++++++++--- .../Physics/OdePlugin/ODERayCastRequestManager.cs | 11 +-- OpenSim/Region/Physics/OdePlugin/OdeScene.cs | 41 ++++++++-- 3 files changed, 118 insertions(+), 24 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index a41c856..b2c2d8a 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -63,6 +63,9 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_isphysical; + public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } } + private int m_expectedCollisionContacts = 0; + /// /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. /// @@ -97,6 +100,9 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_taintAngularLock = Vector3.One; private IntPtr Amotor = IntPtr.Zero; + private object m_assetsLock = new object(); + private bool m_assetFailed = false; + private Vector3 m_PIDTarget; private float m_PIDTau; private float PID_D = 35f; @@ -279,6 +285,7 @@ namespace OpenSim.Region.Physics.OdePlugin } m_taintadd = true; + m_assetFailed = false; _parent_scene.AddPhysicsActorTaint(this); } @@ -601,8 +608,8 @@ namespace OpenSim.Region.Physics.OdePlugin break; case HollowShape.Circle: - // Hollow shape is a perfect cylinder in respect to the cube's scale - // Cylinder hollow volume calculation + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation hollowVolume *= 0.1963495f * 3.07920140172638f; break; @@ -840,7 +847,7 @@ namespace OpenSim.Region.Physics.OdePlugin int vertexStride, triStride; mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage - + m_expectedCollisionContacts = indexCount; mesh.releaseSourceMeshData(); // free up the original mesh data to save memory // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at @@ -1377,6 +1384,7 @@ Console.WriteLine("CreateGeom:"); { //Console.WriteLine(" CreateGeom 1"); SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); + m_expectedCollisionContacts = 3; } catch (AccessViolationException) { @@ -1391,6 +1399,7 @@ Console.WriteLine("CreateGeom:"); { //Console.WriteLine(" CreateGeom 2"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + m_expectedCollisionContacts = 4; } catch (AccessViolationException) { @@ -1406,6 +1415,7 @@ Console.WriteLine("CreateGeom:"); { //Console.WriteLine(" CreateGeom 3"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + m_expectedCollisionContacts = 4; } catch (AccessViolationException) { @@ -1421,6 +1431,7 @@ Console.WriteLine("CreateGeom:"); { //Console.WriteLine(" CreateGeom 4"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + m_expectedCollisionContacts = 4; } catch (AccessViolationException) { @@ -1446,11 +1457,13 @@ Console.WriteLine("CreateGeom:"); _parent_scene.geom_name_map.Remove(prim_geom); _parent_scene.actor_name_map.Remove(prim_geom); d.GeomDestroy(prim_geom); + m_expectedCollisionContacts = 0; prim_geom = IntPtr.Zero; } catch (System.AccessViolationException) { prim_geom = IntPtr.Zero; + m_expectedCollisionContacts = 0; m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name); return false; @@ -1489,6 +1502,8 @@ Console.WriteLine("CreateGeom:"); mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); // createmesh returns null when it's a shape that isn't a cube. // m_log.Debug(m_localID); + if (mesh == null) + CheckMeshAsset(); } #if SPAM @@ -1988,7 +2003,12 @@ Console.WriteLine(" JointCreateFixed"); // Don't need to re-enable body.. it's done in SetMesh if (_parent_scene.needsMeshing(_pbs)) + { mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); + if (mesh == null) + CheckMeshAsset(); + } + } CreateGeom(m_targetSpace, mesh); @@ -2048,6 +2068,8 @@ Console.WriteLine(" JointCreateFixed"); /// private void changeshape() { + m_taintshape = false; + // Cleanup of old prim geometry and Bodies if (IsPhysical && Body != IntPtr.Zero) { @@ -2075,6 +2097,7 @@ Console.WriteLine(" JointCreateFixed"); IMesh mesh = null; + if (_parent_scene.needsMeshing(_pbs)) { // Don't need to re-enable body.. it's done in CreateMesh @@ -2085,6 +2108,8 @@ Console.WriteLine(" JointCreateFixed"); // createmesh returns null when it doesn't mesh. mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); + if (mesh == null) + CheckMeshAsset(); } CreateGeom(m_targetSpace, mesh); @@ -2121,7 +2146,7 @@ Console.WriteLine(" JointCreateFixed"); } resetCollisionAccounting(); - m_taintshape = false; +// m_taintshape = false; } /// @@ -2387,6 +2412,7 @@ Console.WriteLine(" JointCreateFixed"); set { _pbs = value; + m_assetFailed = false; m_taintshape = true; } } @@ -2395,15 +2421,15 @@ Console.WriteLine(" JointCreateFixed"); { get { - // Averate previous velocity with the new one so + // Average previous velocity with the new one so // client object interpolation works a 'little' better if (_zeroFlag) return Vector3.Zero; Vector3 returnVelocity = Vector3.Zero; - returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; - returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; - returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; + returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2' + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f; return returnVelocity; } set @@ -2600,6 +2626,7 @@ Console.WriteLine(" JointCreateFixed"); { Vector3 pv = Vector3.Zero; bool lastZeroFlag = _zeroFlag; + float m_minvelocity = 0; if (Body != (IntPtr)0) // FIXME -> or if it is a joint { d.Vector3 vec = d.BodyGetPosition(Body); @@ -2752,8 +2779,21 @@ Console.WriteLine(" JointCreateFixed"); _acceleration = ((_velocity - m_lastVelocity) / 0.1f); _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); + + // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... + // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. + // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles + // adding these logical exclusion situations to maintain this where I think it was intended to be. + if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) + { + m_minvelocity = 0.5f; + } + else + { + m_minvelocity = 0.02f; + } - if (_velocity.ApproxEquals(pv, 0.5f)) + if (_velocity.ApproxEquals(pv, m_minvelocity)) { m_rotationalVelocity = pv; } @@ -3211,5 +3251,37 @@ Console.WriteLine(" JointCreateFixed"); { m_material = pMaterial; } + + + private void CheckMeshAsset() + { + if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero) + { + m_assetFailed = true; + Util.FireAndForget(delegate + { + RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod; + if (assetProvider != null) + assetProvider(_pbs.SculptTexture, MeshAssetReveived); + }); + } + } + + void MeshAssetReveived(AssetBase asset) + { + if (asset.Data != null && asset.Data.Length > 0) + { + if (!_pbs.SculptEntry) + return; + if (_pbs.SculptTexture.ToString() != asset.ID) + return; + + _pbs.SculptData = new byte[asset.Data.Length]; + asset.Data.CopyTo(_pbs.SculptData, 0); + m_assetFailed = false; + m_taintshape = true; + _parent_scene.AddPhysicsActorTaint(this); + } + } } } \ No newline at end of file diff --git a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs index 7e3ec63..8d7d3b3 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs @@ -137,15 +137,8 @@ namespace OpenSim.Region.Physics.OdePlugin ODERayCastRequest[] reqs = m_PendingRequests.ToArray(); for (int i = 0; i < reqs.Length; i++) { - try - { - if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast - RayCast(reqs[i]); // if there isn't anyone to send results - } - catch - { - //Fail silently - } + if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast + RayCast(reqs[i]); // if there isn't anyone to send results } m_PendingRequests.Clear(); diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs index 929b019..7a50c4c 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs @@ -336,6 +336,7 @@ namespace OpenSim.Region.Physics.OdePlugin public int geomContactPointsStartthrottle = 3; public int geomUpdatesPerThrottledUpdate = 15; + private const int avatarExpectedContacts = 3; public float bodyPIDD = 35f; public float bodyPIDG = 25; @@ -474,6 +475,8 @@ namespace OpenSim.Region.Physics.OdePlugin private OdePrim cp1; private OdeCharacter cc2; private OdePrim cp2; + private int p1ExpectedPoints = 0; + private int p2ExpectedPoints = 0; //private int cStartStop = 0; //private string cDictKey = ""; @@ -498,6 +501,7 @@ namespace OpenSim.Region.Physics.OdePlugin public int physics_logging_interval = 0; public bool physics_logging_append_existing_logfile = false; + public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f); public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f); @@ -644,7 +648,7 @@ namespace OpenSim.Region.Physics.OdePlugin contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); - geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); + geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5); geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); @@ -1064,7 +1068,10 @@ namespace OpenSim.Region.Physics.OdePlugin PhysicsActor p1; PhysicsActor p2; - + + p1ExpectedPoints = 0; + p2ExpectedPoints = 0; + if (!actor_name_map.TryGetValue(g1, out p1)) { p1 = PANull; @@ -1121,9 +1128,13 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p1.PhysicsActorType) { case (int)ActorTypes.Agent: + p1ExpectedPoints = avatarExpectedContacts; p2.CollidingObj = true; break; case (int)ActorTypes.Prim: + if (p1 != null && p1 is OdePrim) + p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts; + if (p2.Velocity.LengthSquared() > 0.0f) p2.CollidingObj = true; break; @@ -1319,6 +1330,7 @@ namespace OpenSim.Region.Physics.OdePlugin if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) { + p2ExpectedPoints = avatarExpectedContacts; // Avatar is moving on terrain, use the movement terrain contact AvatarMovementTerrainContact.geom = curContact; @@ -1332,6 +1344,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (p2.PhysicsActorType == (int)ActorTypes.Agent) { + p2ExpectedPoints = avatarExpectedContacts; // Avatar is standing on terrain, use the non moving terrain contact TerrainContact.geom = curContact; @@ -1356,9 +1369,18 @@ namespace OpenSim.Region.Physics.OdePlugin } if (p2 is OdePrim) - material = ((OdePrim)p2).m_material; - + { + material = ((OdePrim) p2).m_material; + p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; + } + + // Unnessesary because p1 is defined above + //if (p1 is OdePrim) + // { + // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts; + // } //m_log.DebugFormat("Material: {0}", material); + m_materialContacts[material, movintYN].geom = curContact; if (m_global_contactcount < maxContactsbeforedeath) @@ -1379,7 +1401,10 @@ namespace OpenSim.Region.Physics.OdePlugin int material = (int)Material.Wood; if (p2 is OdePrim) + { material = ((OdePrim)p2).m_material; + p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; + } //m_log.DebugFormat("Material: {0}", material); m_materialContacts[material, movintYN].geom = curContact; @@ -1429,6 +1454,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if ((p2.PhysicsActorType == (int)ActorTypes.Agent)) { + p2ExpectedPoints = avatarExpectedContacts; if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) { // Avatar is moving on a prim, use the Movement prim contact @@ -1458,7 +1484,10 @@ namespace OpenSim.Region.Physics.OdePlugin int material = (int)Material.Wood; if (p2 is OdePrim) + { material = ((OdePrim)p2).m_material; + p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; + } //m_log.DebugFormat("Material: {0}", material); m_materialContacts[material, 0].geom = curContact; @@ -1479,8 +1508,8 @@ namespace OpenSim.Region.Physics.OdePlugin } collision_accounting_events(p1, p2, maxDepthContact); - - if (count > geomContactPointsStartthrottle) + + if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle)) { // If there are more then 3 contact points, it's likely // that we've got a pile of objects, so ... -- cgit v1.1 From 4a87a8f3b949d299ca83c99d8f0fda8597187c23 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 7 Oct 2012 06:11:51 +0100 Subject: comment out a spam coment on core Meshmerizer --- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 30 ++++----------------------- 1 file changed, 4 insertions(+), 26 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index 3c4f737..236adb0 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -74,8 +74,6 @@ namespace OpenSim.Region.Physics.Meshing #endif private bool cacheSculptMaps = true; - private bool cacheSculptAlphaMaps = true; - private string decodedSculptMapPath = null; private bool useMeshiesPhysicsMesh = false; @@ -89,16 +87,7 @@ namespace OpenSim.Region.Physics.Meshing IConfig mesh_config = config.Configs["Mesh"]; decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); - cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps); - - if (Environment.OSVersion.Platform == PlatformID.Unix) - { - cacheSculptAlphaMaps = false; - } - else - cacheSculptAlphaMaps = cacheSculptMaps; - if(mesh_config != null) useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); @@ -279,18 +268,15 @@ namespace OpenSim.Region.Physics.Meshing { if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) return null; - // Remove the reference to any JPEG2000 sculpt data so it can be GCed - // don't loose it - // primShape.SculptData = Utils.EmptyBytes; } -// primShape.SculptDataLoaded = true; } else { if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) return null; } - // keep compatible + + // Remove the reference to any JPEG2000 sculpt data so it can be GCed primShape.SculptData = Utils.EmptyBytes; int numCoords = coords.Count; @@ -335,7 +321,7 @@ namespace OpenSim.Region.Physics.Meshing if (primShape.SculptData.Length <= 0) { - m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName); +// m_log.ErrorFormat("[MESH]: asset data for {0} is zero length", primName); return false; } @@ -496,8 +482,7 @@ namespace OpenSim.Region.Physics.Meshing //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData); - if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0))) - // don't cache images with alpha channel in linux since mono can't load them correctly) + if (cacheSculptMaps) { try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); } catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); } @@ -717,11 +702,6 @@ namespace OpenSim.Region.Physics.Meshing return CreateMesh(primName, primShape, size, lod, false); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) - { - return CreateMesh(primName, primShape, size, lod, false); - } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { #if SPAM @@ -763,7 +743,5 @@ namespace OpenSim.Region.Physics.Meshing return mesh; } - public void ReleaseMesh(IMesh imesh) { } - public void ExpireReleaseMeshs() { } } } -- cgit v1.1 From c42df1259fa02aea21817749d65a4e468166c022 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 7 Oct 2012 06:16:47 +0100 Subject: fix wrong file commited --- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 10 ---------- 1 file changed, 10 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index 7a3f343..236adb0 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -743,15 +743,5 @@ namespace OpenSim.Region.Physics.Meshing return mesh; } -<<<<<<< HEAD - public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) - { - return null; - } - - public void ReleaseMesh(IMesh imesh) { } - public void ExpireReleaseMeshs() { } -======= ->>>>>>> avination } } -- cgit v1.1 From a0b4e68060b51deba6262b0c81873ce734cb0089 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 7 Oct 2012 06:33:13 +0100 Subject: refix so we can compile it, loosing alpha scultps fix on core meshmerizer --- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 11 +++++++++++ 1 file changed, 11 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index 236adb0..b462713 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -702,6 +702,10 @@ namespace OpenSim.Region.Physics.Meshing return CreateMesh(primName, primShape, size, lod, false); } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + return CreateMesh(primName, primShape, size, lod, false); + } public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { #if SPAM @@ -743,5 +747,12 @@ namespace OpenSim.Region.Physics.Meshing return mesh; } + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + return null; + } + + public void ReleaseMesh(IMesh imesh) { } + public void ExpireReleaseMeshs() { } } } -- cgit v1.1 From 48d8fbc9aedb3247a1dfd25be1b7dfbdd8719790 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 7 Oct 2012 08:53:55 +0100 Subject: bug fix + make costs visible for testing --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 17 +++++++ .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 40 ++++++++-------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 56 +++++++++++++++------- 3 files changed, 76 insertions(+), 37 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index a2c72c3..14f65b8 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -315,6 +315,23 @@ namespace OpenSim.Region.Physics.Manager } } + + public virtual float PhysicsCost + { + get + { + return 0.1f; + } + } + + public virtual float StreamCost + { + get + { + return 1.0f; + } + } + /// /// Velocity of this actor. /// diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 3fcbb1b..6bdc089 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -40,8 +40,6 @@ namespace OpenSim.Region.Physics.OdePlugin public float volume; - public float physCost; - public float streamCost; public byte shapetype; public bool hasOBB; public bool hasMeshVolume; @@ -121,8 +119,14 @@ namespace OpenSim.Region.Physics.OdePlugin public void Stop() { - m_running = false; - m_thread.Abort(); + try + { + m_thread.Abort(); + createqueue.Clear(); + } + catch + { + } } public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, @@ -172,10 +176,15 @@ namespace OpenSim.Region.Physics.OdePlugin if (repData.assetState != AssetState.needAsset) return; + repData.mesh = null; + if (repData.assetID == null || repData.assetID == UUID.Zero) + { + repData.assetState = AssetState.noNeedAsset; + repData.comand = meshWorkerCmnds.changefull; + createqueue.Enqueue(repData); return; - - repData.mesh = null; + } repData.assetState = AssetState.loadingAsset; @@ -211,9 +220,6 @@ namespace OpenSim.Region.Physics.OdePlugin repData.OBBOffset = mesh.GetCentroid(); repData.OBB = mesh.GetOBB(); repData.hasOBB = true; - repData.physCost = 0.0013f * (float)indexCount; - // todo - repData.streamCost = 1.0f; mesh.releaseSourceMeshData(); } } @@ -427,11 +433,14 @@ namespace OpenSim.Region.Physics.OdePlugin if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero) { repData.assetID = pbs.SculptTexture; - repData.assetState = AssetState.needAsset; + repData.assetState = AssetState.needAsset; } else repData.assetState = AssetState.AssetFailed; } + else + repData.assetState = AssetState.needAsset; + return false; } @@ -828,14 +837,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (repData.hasOBB) { Vector3 OBB = repData.OBB; - float pc = repData.physCost; - float psf = OBB.X * (OBB.Y + OBB.Z) + OBB.Y * OBB.Z; - psf *= 1.33f * .2f; - pc *= psf; - if (pc < 0.1f) - pc = 0.1f; - - repData.physCost = pc; } else { @@ -846,9 +847,6 @@ namespace OpenSim.Region.Physics.OdePlugin repData.OBB = OBB; repData.OBBOffset = Vector3.Zero; - - repData.physCost = 0.1f; - repData.streamCost = 1.0f; } CalculateBasicPrimVolume(repData); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index cbe129a..4c16f8e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -277,7 +277,23 @@ namespace OpenSim.Region.Physics.OdePlugin cdata.mu *= veh.FrictionFactor; // cdata.mu *= 0; } - } + } + + public override float PhysicsCost + { + get + { + return m_physCost; + } + } + + public override float StreamCost + { + get + { + return m_streamCost; + } + } public override int PhysicsActorType { @@ -1373,6 +1389,11 @@ namespace OpenSim.Region.Physics.OdePlugin m_mesh = null; return false; } + + m_physCost = 0.0013f * (float)indexCount; + // todo + m_streamCost = 1.0f; + return true; } @@ -1386,7 +1407,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_assetState == AssetState.AssetFailed) m_NoColide = true; - else if (m_mesh != null) + else if(m_mesh != null) { if (GetMeshGeom()) hasMesh = true; @@ -2058,7 +2079,23 @@ namespace OpenSim.Region.Physics.OdePlugin m_OBBOffset.Y, m_OBBOffset.Z); - primOOBradiusSQ = m_OBBOffset.LengthSquared(); + primOOBradiusSQ = m_OBB.LengthSquared(); + + if (_triMeshData != IntPtr.Zero) + { + float pc = m_physCost; + float psf = primOOBradiusSQ; + psf *= 1.33f * .2f; + pc *= psf; + if (pc < 0.1f) + pc = 0.1f; + + m_physCost = pc; + } + else + m_physCost = 0.1f; + + m_streamCost = 1.0f; } #endregion @@ -2717,10 +2754,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_OBBOffset = repData.OBBOffset; m_OBB = repData.OBB; -// m_NoColide = repData.NoColide; - m_physCost = repData.physCost; - m_streamCost = repData.streamCost; - primVolume = repData.volume; CreateGeom(); @@ -2793,9 +2826,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_OBBOffset = repData.OBBOffset; m_OBB = repData.OBB; - m_physCost = repData.physCost; - m_streamCost = repData.streamCost; - primVolume = repData.volume; CreateGeom(); @@ -2806,14 +2836,8 @@ namespace OpenSim.Region.Physics.OdePlugin UpdatePrimBodyData(); - try - { _parent_scene.actor_name_map[prim_geom] = this; - } - catch - { - } d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); d.Quaternion myrot = new d.Quaternion(); -- cgit v1.1 From a1fcfe867786de121d0960e684252525781ac453 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 7 Oct 2012 23:54:15 +0100 Subject: a few changes/fix (?) --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 18 +-- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 150 +++++++++++---------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 50 +++---- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 11 -- 4 files changed, 112 insertions(+), 117 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 2933d86..fabadd3 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -1113,17 +1113,17 @@ namespace OpenSim.Region.Physics.Meshing Mesh mesh = (Mesh)imesh; - int curRefCount = mesh.RefCount; - curRefCount--; - - if (curRefCount > 0) - { - mesh.RefCount = curRefCount; - return; - } - lock (m_uniqueMeshes) { + int curRefCount = mesh.RefCount; + curRefCount--; + + if (curRefCount > 0) + { + mesh.RefCount = curRefCount; + return; + } + mesh.RefCount = 0; m_uniqueMeshes.Remove(mesh.Key); lock (m_uniqueReleasedMeshes) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 6bdc089..024835c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -18,6 +18,23 @@ using OpenMetaverse; namespace OpenSim.Region.Physics.OdePlugin { + public enum MeshState : byte + { + noNeed = 0, + + loadingAsset = 1, + + AssetOK = 0x0f, // 00001111 + + NeedMask = 0x30, // 00110000 + needMesh = 0x10, // 00010000 + needAsset = 0x20, // 00100000 + + FailMask = 0xC0, // 11000000 + AssetFailed = 0x40, // 01000000 + MeshFailed = 0x80 // 10000000 + } + public enum meshWorkerCmnds : byte { nop = 0, @@ -43,13 +60,11 @@ namespace OpenSim.Region.Physics.OdePlugin public byte shapetype; public bool hasOBB; public bool hasMeshVolume; - public AssetState assetState; + public MeshState meshState; public UUID? assetID; public meshWorkerCmnds comand; } - - public class ODEMeshWorker { @@ -138,16 +153,10 @@ namespace OpenSim.Region.Physics.OdePlugin repData.size = size; repData.shapetype = shapetype; - // if (CheckMeshDone(repData)) - { - CheckMeshDone(repData); - CalcVolumeData(repData); - m_scene.AddChange(actor, changes.PhysRepData, repData); - return; - } - -// repData.comand = meshWorkerCmnds.changefull; -// createqueue.Enqueue(repData); + CheckMeshDone(repData); + CalcVolumeData(repData); + m_scene.AddChange(actor, changes.PhysRepData, repData); + return; } public void NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, @@ -159,39 +168,43 @@ namespace OpenSim.Region.Physics.OdePlugin repData.size = size; repData.shapetype = shapetype; - // bool done = CheckMeshDone(repData); - CheckMeshDone(repData); CalcVolumeData(repData); m_scene.AddChange(actor, changes.AddPhysRep, repData); -// if (done) - return; - -// repData.comand = meshWorkerCmnds.addnew; -// createqueue.Enqueue(repData); } - public void RequestMeshAsset(ODEPhysRepData repData) + public void RequestMesh(ODEPhysRepData repData) { - if (repData.assetState != AssetState.needAsset) - return; - repData.mesh = null; - if (repData.assetID == null || repData.assetID == UUID.Zero) + if (repData.meshState == MeshState.needMesh) { - repData.assetState = AssetState.noNeedAsset; repData.comand = meshWorkerCmnds.changefull; createqueue.Enqueue(repData); - return; } + else if (repData.meshState == MeshState.needAsset) + { + PrimitiveBaseShape pbs = repData.pbs; + + // check if we got outdated + + if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero) + { + repData.meshState = MeshState.noNeed; + return; + } + + if (pbs.SculptTexture != repData.assetID) + return; - repData.assetState = AssetState.loadingAsset; + repData.meshState = MeshState.loadingAsset; - repData.comand = meshWorkerCmnds.getmesh; - createqueue.Enqueue(repData); + repData.comand = meshWorkerCmnds.getmesh; + createqueue.Enqueue(repData); + } } + // creates and prepares a mesh to use and calls parameters estimation public bool CreateActorPhysRep(ODEPhysRepData repData) { getMesh(repData); @@ -210,7 +223,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}", repData.actor.Name, repData.pbs.SculptTexture.ToString()); - repData.assetState = AssetState.AssetFailed; + repData.meshState = MeshState.MeshFailed; repData.hasOBB = false; repData.mesh = null; m_scene.mesher.ReleaseMesh(mesh); @@ -237,6 +250,8 @@ namespace OpenSim.Region.Physics.OdePlugin createqueue.Enqueue(repData); } } + else + repData.pbs.SculptData = Utils.EmptyBytes; } public void DoRepDataGetMesh(ODEPhysRepData repData) @@ -244,7 +259,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!repData.pbs.SculptEntry) return; - if (repData.assetState != AssetState.loadingAsset) + if (repData.meshState != MeshState.loadingAsset) return; if (repData.assetID == null || repData.assetID == UUID.Zero) @@ -381,34 +396,19 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } + // see if we need a mesh and if so if we have a cached one + // called with a new repData public bool CheckMeshDone(ODEPhysRepData repData) { PhysicsActor actor = repData.actor; PrimitiveBaseShape pbs = repData.pbs; - repData.mesh = null; - repData.hasOBB = false; - if (!needsMeshing(pbs)) { - repData.assetState = AssetState.noNeedAsset; + repData.meshState = MeshState.noNeed; return true; } - if (pbs.SculptEntry) - { - if (repData.assetState == AssetState.AssetFailed) - { - if (pbs.SculptTexture == repData.assetID) - return true; - } - } - else - { - repData.assetState = AssetState.noNeedAsset; - repData.assetID = null; - } - IMesh mesh = null; Vector3 size = repData.size; @@ -425,7 +425,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (pbs.SculptType != (byte)SculptType.Mesh) clod = (int)LevelOfDetail.Low; } + mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); + if (mesh == null) { if (pbs.SculptEntry) @@ -433,13 +435,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero) { repData.assetID = pbs.SculptTexture; - repData.assetState = AssetState.needAsset; + repData.meshState = MeshState.needAsset; } else - repData.assetState = AssetState.AssetFailed; + repData.meshState = MeshState.MeshFailed; } else - repData.assetState = AssetState.needAsset; + repData.meshState = MeshState.needMesh; return false; } @@ -447,14 +449,14 @@ namespace OpenSim.Region.Physics.OdePlugin repData.mesh = mesh; if (pbs.SculptEntry) { - repData.assetState = AssetState.AssetOK; + repData.meshState = MeshState.AssetOK; repData.assetID = pbs.SculptTexture; - pbs.SculptData = Utils.EmptyBytes; } + + pbs.SculptData = Utils.EmptyBytes; return true; } - public bool getMesh(ODEPhysRepData repData) { PhysicsActor actor = repData.actor; @@ -467,16 +469,19 @@ namespace OpenSim.Region.Physics.OdePlugin if (!needsMeshing(pbs)) return false; + if (repData.meshState == MeshState.MeshFailed) + return false; + if (pbs.SculptEntry) { - if (repData.assetState == AssetState.AssetFailed) + if (repData.meshState == MeshState.AssetFailed) { if (pbs.SculptTexture == repData.assetID) return true; } } - repData.assetState = AssetState.noNeedAsset; + repData.meshState = MeshState.noNeed; IMesh mesh = null; Vector3 size = repData.size; @@ -492,7 +497,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (pbs.SculptType != (byte)SculptType.Mesh) clod = (int)LevelOfDetail.Low; } + + // check cached mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); + if (mesh == null) { if (pbs.SculptEntry) @@ -501,17 +509,16 @@ namespace OpenSim.Region.Physics.OdePlugin return false; repData.assetID = pbs.SculptTexture; - repData.assetState = AssetState.AssetOK; + repData.meshState = MeshState.AssetOK; if (pbs.SculptData == null || pbs.SculptData.Length == 0) { - repData.assetState = AssetState.needAsset; + repData.meshState = MeshState.needAsset; return false; } } mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); - } repData.mesh = mesh; @@ -520,13 +527,14 @@ namespace OpenSim.Region.Physics.OdePlugin if (mesh == null) { if (pbs.SculptEntry) - repData.assetState = AssetState.AssetFailed; + repData.meshState = MeshState.AssetFailed; + else + repData.meshState = MeshState.MeshFailed; return false; } - if (pbs.SculptEntry) - repData.assetState = AssetState.AssetOK; + repData.meshState = MeshState.AssetOK; return true; } @@ -866,7 +874,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_log = plog; repData = pRepData; - repData.assetState = AssetState.AssetFailed; + repData.meshState = MeshState.AssetFailed; if (provider == null) return; @@ -877,29 +885,27 @@ namespace OpenSim.Region.Physics.OdePlugin if (assetID == UUID.Zero) return; - repData.assetState = AssetState.loadingAsset; + repData.meshState = MeshState.loadingAsset; provider(assetID, ODEassetReceived); } void ODEassetReceived(AssetBase asset) { - repData.assetState = AssetState.AssetFailed; + repData.meshState = MeshState.AssetFailed; if (asset != null) { if (asset.Data != null && asset.Data.Length > 0) { + repData.meshState = MeshState.noNeed; + if (!repData.pbs.SculptEntry) return; if (repData.pbs.SculptTexture != repData.assetID) return; - // asset get may return a pointer to the same asset data - // for similar prims and we destroy with it - // so waste a lot of time stressing gc and hoping it clears things - // TODO avoid this repData.pbs.SculptData = new byte[asset.Data.Length]; asset.Data.CopyTo(repData.pbs.SculptData,0); - repData.assetState = AssetState.AssetOK; + repData.meshState = MeshState.AssetOK; m_worker.AssetLoaded(repData); } else diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 4c16f8e..c2c4384 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -146,7 +146,7 @@ namespace OpenSim.Region.Physics.OdePlugin private PrimitiveBaseShape _pbs; private UUID? m_assetID; - private AssetState m_assetState; + private MeshState m_meshState; public OdeScene _parent_scene; @@ -1341,15 +1341,18 @@ namespace OpenSim.Region.Physics.OdePlugin if (vertexCount == 0 || indexCount == 0) { - m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0} mesh UUID {1}", - Name, _pbs.SculptTexture.ToString()); + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1}", + Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh"); + m_hasOBB = false; m_OBBOffset = Vector3.Zero; m_OBB = _size * 0.5f; + m_physCost = 0.1f; m_streamCost = 1.0f; + _parent_scene.mesher.ReleaseMesh(mesh); - m_assetState = AssetState.AssetFailed; + m_meshState = MeshState.MeshFailed; m_mesh = null; return false; } @@ -1360,7 +1363,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); d.GeomTriMeshDataPreprocess(_triMeshData); - prim_geom = d.CreateTriMesh(IntPtr.Zero, _triMeshData, null, null, null); + prim_geom = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); } catch (Exception e) @@ -1385,7 +1388,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_physCost = 0.1f; m_streamCost = 1.0f; _parent_scene.mesher.ReleaseMesh(mesh); - m_assetState = AssetState.AssetFailed; + m_meshState = MeshState.AssetFailed; m_mesh = null; return false; } @@ -1404,7 +1407,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_NoColide = false; - if (m_assetState == AssetState.AssetFailed) + if ((m_meshState & MeshState.FailMask) != 0) m_NoColide = true; else if(m_mesh != null) @@ -1422,7 +1425,7 @@ namespace OpenSim.Region.Physics.OdePlugin { // it's a sphere try { - geo = d.CreateSphere(IntPtr.Zero, _size.X * 0.5f); + geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); } catch (Exception e) { @@ -1434,7 +1437,7 @@ namespace OpenSim.Region.Physics.OdePlugin {// do it as a box try { - geo = d.CreateBox(IntPtr.Zero, _size.X, _size.Y, _size.Z); + geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); } catch (Exception e) { @@ -2748,7 +2751,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_mesh = repData.mesh; m_assetID = repData.assetID; - m_assetState = repData.assetState; + m_meshState = repData.meshState; m_hasOBB = repData.hasOBB; m_OBBOffset = repData.OBBOffset; @@ -2781,12 +2784,12 @@ namespace OpenSim.Region.Physics.OdePlugin else MakeBody(); - if (m_assetState == AssetState.needAsset) + if ((m_meshState & MeshState.NeedMask) != 0) { repData.size = _size; repData.pbs = _pbs; repData.shapetype = m_shapetype; - _parent_scene.m_meshWorker.RequestMeshAsset(repData); + _parent_scene.m_meshWorker.RequestMesh(repData); } } } @@ -2820,7 +2823,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_mesh = repData.mesh; m_assetID = repData.assetID; - m_assetState = repData.assetState; + m_meshState = repData.meshState; m_hasOBB = repData.hasOBB; m_OBBOffset = repData.OBBOffset; @@ -2832,12 +2835,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (prim_geom != IntPtr.Zero) { - m_targetSpace = IntPtr.Zero; - UpdatePrimBodyData(); - _parent_scene.actor_name_map[prim_geom] = this; - + _parent_scene.actor_name_map[prim_geom] = this; d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); d.Quaternion myrot = new d.Quaternion(); @@ -2847,7 +2847,6 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.W = _orientation.W; d.GeomSetQuaternion(prim_geom, ref myrot); - if (m_isphysical) { if (chp) @@ -2869,13 +2868,14 @@ namespace OpenSim.Region.Physics.OdePlugin } resetCollisionAccounting(); - if (m_assetState == AssetState.needAsset) - { - repData.size = _size; - repData.pbs = _pbs; - repData.shapetype = m_shapetype; - _parent_scene.m_meshWorker.RequestMeshAsset(repData); - } + } + + if ((m_meshState & MeshState.NeedMask) != 0) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_shapetype; + _parent_scene.m_meshWorker.RequestMesh(repData); } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 5e4c2a5..ed2a531 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -111,17 +111,6 @@ namespace OpenSim.Region.Physics.OdePlugin light = 7 // compatibility with old viewers } - public enum AssetState : byte - { - noNeedAsset = 0, - needAsset = 1, - loadingAsset = 2, - procAsset = 3, - AssetOK = 4, - - AssetFailed = 0xff - } - public enum changes : int { Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) -- cgit v1.1 From 3bf7201fd4d3e2b240f8139c20f88c916d338c31 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 00:18:39 +0100 Subject: move terrain geom to own ode space. Limit range on raycast if includes terrain until ode doesn't eat all stack. Add a pre-simulation method to do pending actors changes (except mesh assets still not ready to use), to be optionaly called before firing heartbeat. [UNTESTED] --- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 3 + OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 128 ++++++++++------ .../UbitOdePlugin/ODERayCastRequestManager.cs | 16 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 165 ++++++++++++++++----- 4 files changed, 229 insertions(+), 83 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 46bfa59..ce269fa 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -246,6 +246,9 @@ namespace OpenSim.Region.Physics.Manager public abstract void AddPhysicsActorTaint(PhysicsActor prim); + + public virtual void PrepareSimulation() { } + /// /// Perform a simulation of the current physics scene over the given timestep. /// diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index c2c4384..535a4e2 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1325,12 +1325,48 @@ namespace OpenSim.Region.Physics.OdePlugin } } + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + { + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); + } + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + } + + UpdatePrimBodyData(); + _parent_scene.actor_name_map[prim_geom] = this; + + } + else + m_log.Warn("Setting bad Geom"); + } + private bool GetMeshGeom() { IntPtr vertices, indices; int vertexCount, indexCount; int vertexStride, triStride; + IMesh mesh = m_mesh; if (mesh == null) @@ -1356,6 +1392,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_mesh = null; return false; } + + IntPtr geo = IntPtr.Zero; + try { _triMeshData = d.GeomTriMeshDataCreate(); @@ -1363,7 +1402,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); d.GeomTriMeshDataPreprocess(_triMeshData); - prim_geom = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); } catch (Exception e) @@ -1380,15 +1419,15 @@ namespace OpenSim.Region.Physics.OdePlugin } } _triMeshData = IntPtr.Zero; - prim_geom = IntPtr.Zero; m_hasOBB = false; m_OBBOffset = Vector3.Zero; m_OBB = _size * 0.5f; m_physCost = 0.1f; - m_streamCost = 1.0f; + m_streamCost = 1.0f; + _parent_scene.mesher.ReleaseMesh(mesh); - m_meshState = MeshState.AssetFailed; + m_meshState = MeshState.MeshFailed; m_mesh = null; return false; } @@ -1397,12 +1436,13 @@ namespace OpenSim.Region.Physics.OdePlugin // todo m_streamCost = 1.0f; + SetGeom(geo); + return true; } private void CreateGeom() { - IntPtr geo = IntPtr.Zero; bool hasMesh = false; m_NoColide = false; @@ -1418,8 +1458,11 @@ namespace OpenSim.Region.Physics.OdePlugin m_NoColide = true; } + if (!hasMesh) { + IntPtr geo = IntPtr.Zero; + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 && _size.X == _size.Y && _size.Y == _size.Z) { // it's a sphere @@ -1447,7 +1490,7 @@ namespace OpenSim.Region.Physics.OdePlugin } m_physCost = 0.1f; m_streamCost = 1.0f; - prim_geom = geo; + SetGeom(geo); } } @@ -2740,8 +2783,6 @@ namespace OpenSim.Region.Physics.OdePlugin { } - - private void changeAddPhysRep(ODEPhysRepData repData) { _size = repData.size; //?? @@ -2763,10 +2804,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (prim_geom != IntPtr.Zero) { - UpdatePrimBodyData(); - - _parent_scene.actor_name_map[prim_geom] = this; - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); d.Quaternion myrot = new d.Quaternion(); myrot.X = _orientation.X; @@ -2774,23 +2811,23 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.Z = _orientation.Z; myrot.W = _orientation.W; d.GeomSetQuaternion(prim_geom, ref myrot); + } - if (!m_isphysical) - { - SetInStaticSpace(this); - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - else - MakeBody(); + if (!m_isphysical) + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + else + MakeBody(); - if ((m_meshState & MeshState.NeedMask) != 0) - { - repData.size = _size; - repData.pbs = _pbs; - repData.shapetype = m_shapetype; - _parent_scene.m_meshWorker.RequestMesh(repData); - } + if ((m_meshState & MeshState.NeedMask) != 0) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_shapetype; + _parent_scene.m_meshWorker.RequestMesh(repData); } } @@ -2831,14 +2868,10 @@ namespace OpenSim.Region.Physics.OdePlugin primVolume = repData.volume; - CreateGeom(); + CreateGeom(); if (prim_geom != IntPtr.Zero) { - UpdatePrimBodyData(); - - _parent_scene.actor_name_map[prim_geom] = this; - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); d.Quaternion myrot = new d.Quaternion(); myrot.X = _orientation.X; @@ -2846,29 +2879,28 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.Z = _orientation.Z; myrot.W = _orientation.W; d.GeomSetQuaternion(prim_geom, ref myrot); + } - if (m_isphysical) + if (m_isphysical) + { + if (chp) { - if (chp) + if (parent != null) { - if (parent != null) - { - parent.MakeBody(); - } + parent.MakeBody(); } - else - MakeBody(); } - else - { - SetInStaticSpace(this); - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - - resetCollisionAccounting(); + MakeBody(); } + else + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + + resetCollisionAccounting(); if ((m_meshState & MeshState.NeedMask) != 0) { diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 21fe9c0..06cb302 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -129,7 +129,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.length = length; req.Normal = direction; req.Origin = position; - req.filter = RayFilterFlags.AllPrims; + req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; m_PendingRequests.Enqueue(req); } @@ -261,6 +261,12 @@ namespace OpenSim.Region.Physics.OdePlugin closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); + // current ode land to ray collisions is very bad + // so for now limit its range badly + + if (req.length > 30.0f && (CurrentRayFilter & RayFilterFlags.land) != 0) + req.length = 30.0f; + d.GeomRaySetLength(ray, req.length); d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); d.GeomRaySetParams(ray, 0, backfacecull); @@ -288,7 +294,10 @@ namespace OpenSim.Region.Physics.OdePlugin catflags |= CollisionCategories.Water; if (catflags != 0) + { + d.GeomSetCollideBits(ray, (uint)catflags); doSpaceRay(req); + } } else { @@ -314,7 +323,8 @@ namespace OpenSim.Region.Physics.OdePlugin /// private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton; - private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; +// private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; + private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; private void doSpaceRay(ODERayRequest req) { @@ -323,6 +333,8 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback); if (req.callbackMethod is RaycastCallback) { diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index ed2a531..beaba13 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -300,6 +300,7 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr TopSpace; // the global space public IntPtr ActiveSpace; // space for active prims public IntPtr StaticSpace; // space for the static things around + public IntPtr GroundSpace; // space for ground // some speedup variables private int spaceGridMaxX; @@ -372,6 +373,7 @@ namespace OpenSim.Region.Physics.OdePlugin // now the major subspaces ActiveSpace = d.HashSpaceCreate(TopSpace); StaticSpace = d.HashSpaceCreate(TopSpace); + GroundSpace = d.HashSpaceCreate(TopSpace); } catch { @@ -381,10 +383,12 @@ namespace OpenSim.Region.Physics.OdePlugin d.HashSpaceSetLevels(TopSpace, -2, 8); d.HashSpaceSetLevels(ActiveSpace, -2, 8); d.HashSpaceSetLevels(StaticSpace, -2, 8); + d.HashSpaceSetLevels(GroundSpace, 0, 8); // demote to second level d.SpaceSetSublevel(ActiveSpace, 1); d.SpaceSetSublevel(StaticSpace, 1); + d.SpaceSetSublevel(GroundSpace, 1); d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | CollisionCategories.Geom | @@ -402,6 +406,8 @@ namespace OpenSim.Region.Physics.OdePlugin )); d.GeomSetCollideBits(StaticSpace, 0); + d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundSpace, 0); contactgroup = d.JointGroupCreate(0); //contactgroup @@ -1210,6 +1216,7 @@ namespace OpenSim.Region.Physics.OdePlugin chr.CollidingObj = false; // do colisions with static space d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback); + // no coll with gnd } } catch (AccessViolationException) @@ -1238,7 +1245,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (!prm.m_outbounds) { if (d.BodyIsEnabled(prm.Body)) + { d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); + d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); + } } } } @@ -1595,8 +1605,52 @@ namespace OpenSim.Region.Physics.OdePlugin /// /// public override void AddPhysicsActorTaint(PhysicsActor prim) + { + } + + // does all pending changes generated during region load process + public override void PrepareSimulation() + { + lock (OdeLock) { + if (world == IntPtr.Zero) + { + ChangesQueue.Clear(); + return; + } + + ODEchangeitem item; + + int donechanges = 0; + if (ChangesQueue.Count > 0) + { + m_log.InfoFormat("[ODE] start processing pending actor operations"); + int tstart = Util.EnvironmentTickCount(); + + while (ChangesQueue.Dequeue(out item)) + { + if (item.actor != null) + { + try + { + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); + } + catch + { + m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); + } + } + donechanges++; + } + int time = Util.EnvironmentTickCountSubtract(tstart); + m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); + } } + } /// /// This is our main simulate loop @@ -1642,7 +1696,40 @@ namespace OpenSim.Region.Physics.OdePlugin lock(OdeLock) { if (world == IntPtr.Zero) + { + ChangesQueue.Clear(); return 0; + } + + ODEchangeitem item; + + if (ChangesQueue.Count > 0) + { + int ttmpstart = Util.EnvironmentTickCount(); + int ttmp; + + while (ChangesQueue.Dequeue(out item)) + { + if (item.actor != null) + { + try + { + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); + } + catch + { + m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); + } + } + ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp > 20) + break; + } + } d.WorldSetQuickStepNumIterations(world, curphysiteractions); @@ -1653,35 +1740,6 @@ namespace OpenSim.Region.Physics.OdePlugin // clear pointer/counter to contacts to pass into joints m_global_contactcount = 0; - ODEchangeitem item; - - if(ChangesQueue.Count >0) - { - int ttmpstart = Util.EnvironmentTickCount(); - int ttmp; - - while(ChangesQueue.Dequeue(out item)) - { - if (item.actor != null) - { - try - { - if (item.actor is OdeCharacter) - ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); - else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) - RemovePrimThreadLocked((OdePrim)item.actor); - } - catch - { - m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", - item.actor.Name, item.what.ToString()); - } - } - ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); - if (ttmp > 20) - break; - } - } // Move characters lock (_characters) @@ -1813,10 +1871,47 @@ namespace OpenSim.Region.Physics.OdePlugin mesher.ExpireReleaseMeshs(); m_lastMeshExpire = now; } + +// information block running in debug only /* - int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); - int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); + int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); + int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); + int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); + + int nactivegeoms = 0; + int nactivespaces = 0; + + int nstaticgeoms = 0; + int nstaticspaces = 0; + IntPtr sp; + + for (int i = 0; i < ntopactivegeoms; i++) + { + sp = d.SpaceGetGeom(ActiveSpace, i); + if (d.GeomIsSpace(sp)) + { + nactivespaces++; + nactivegeoms += d.SpaceGetNumGeoms(sp); + } + else + nactivegeoms++; + } + + for (int i = 0; i < ntopstaticgeoms; i++) + { + sp = d.SpaceGetGeom(StaticSpace, i); + if (d.GeomIsSpace(sp)) + { + nstaticspaces++; + nstaticgeoms += d.SpaceGetNumGeoms(sp); + } + else + nstaticgeoms++; + } + int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); + + int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray int nbodies = d.NTotalBodies; int ngeoms = d.NTotalGeoms; */ @@ -2113,7 +2208,9 @@ namespace OpenSim.Region.Physics.OdePlugin offset, thickness, wrap); d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); - GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); + + GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) { d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); @@ -2234,12 +2331,13 @@ namespace OpenSim.Region.Physics.OdePlugin thickness, wrap); // d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); - GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1); + GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1); if (GroundGeom != IntPtr.Zero) { d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); d.GeomSetCollideBits(GroundGeom, 0); + PhysicsActor pa = new NullPhysicsActor(); pa.Name = "Terrain"; pa.PhysicsActorType = (int)ActorTypes.Ground; @@ -2455,6 +2553,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomDestroy(GroundGeom); } + RegionTerrain.Clear(); if (TerrainHeightFieldHeightsHandlers.Count > 0) -- cgit v1.1 From 2e223c8ce2ccceeca87ae18522a3370e2a5565c9 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 00:44:02 +0100 Subject: Change ray to land colision range limitation so it has no impact on other geom types --- .../Physics/UbitOdePlugin/ODERayCastRequestManager.cs | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 06cb302..799a324 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -261,12 +261,6 @@ namespace OpenSim.Region.Physics.OdePlugin closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); - // current ode land to ray collisions is very bad - // so for now limit its range badly - - if (req.length > 30.0f && (CurrentRayFilter & RayFilterFlags.land) != 0) - req.length = 30.0f; - d.GeomRaySetLength(ray, req.length); d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); d.GeomRaySetParams(ray, 0, backfacecull); @@ -334,7 +328,15 @@ namespace OpenSim.Region.Physics.OdePlugin if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + { + // current ode land to ray collisions is very bad + // so for now limit its range badly + + if (req.length > 30.0f) + d.GeomRaySetLength(ray, 30.0f); + d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + } if (req.callbackMethod is RaycastCallback) { -- cgit v1.1 From 4c512ada58532f97baa7a356cec5a2e2720d7466 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 01:04:29 +0100 Subject: fire a extra terseupdate when stopping (like in loosing physics). In some cases things seem not to stop --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 3 +++ 1 file changed, 3 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 535a4e2..1beb761 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2401,6 +2401,9 @@ namespace OpenSim.Region.Physics.OdePlugin _target_velocity = Vector3.Zero; if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Stop(); + + _zeroFlag = false; + base.RequestPhysicsterseUpdate(); } if (Body != IntPtr.Zero) -- cgit v1.1 From 5ef48c59808743ca36c0d768bce2d0fbbe8f4c5a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 02:57:51 +0100 Subject: temporary debug code --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 1beb761..fc59180 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1355,6 +1355,25 @@ namespace OpenSim.Region.Physics.OdePlugin UpdatePrimBodyData(); _parent_scene.actor_name_map[prim_geom] = this; + +// debug + d.AABB aabb; + d.GeomGetAABB(prim_geom, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if( x > 60.0f || y > 60.0f || z > 60.0f) + m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", + Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); + else if (x < 0.001f || y < 0.001f || z < 0.001f) + m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", + Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); + +// + + + + } else m_log.Warn("Setting bad Geom"); -- cgit v1.1 From 87175412882e8e4b7cd9d92d6a9be8546ec7e9e9 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 04:31:50 +0100 Subject: force allocation of mesh data on creation ( messy code version ) --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 4 ++-- OpenSim/Region/Physics/Manager/IMesher.cs | 2 +- OpenSim/Region/Physics/Manager/ZeroMesher.cs | 2 +- OpenSim/Region/Physics/Meshing/Mesh.cs | 1 + OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 3 ++- OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 26 ++++++++++++++++++++++ OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 16 ++++++++----- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 1 - 9 files changed, 45 insertions(+), 12 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 5b743e8..8de70ef 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -2190,7 +2190,7 @@ namespace OpenSim.Region.Physics.OdePlugin convex = false; try { - _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,convex); + _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,convex,false); } catch { @@ -2557,7 +2557,7 @@ namespace OpenSim.Region.Physics.OdePlugin try { - mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, convex); + mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, convex,false); } catch { diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index 460b48e..fdba6ee 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -37,7 +37,7 @@ namespace OpenSim.Region.Physics.Manager { IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod); IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical); - IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex); + IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde); IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex); void ReleaseMesh(IMesh mesh); void ExpireReleaseMeshs(); diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs index 61da9f3..1411165 100644 --- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs +++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs @@ -67,7 +67,7 @@ namespace OpenSim.Region.Physics.Manager return CreateMesh(primName, primShape, size, lod, false); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex,bool forOde) { return CreateMesh(primName, primShape, size, lod, false); } diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs index c03f18b..6970553 100644 --- a/OpenSim/Region/Physics/Meshing/Mesh.cs +++ b/OpenSim/Region/Physics/Meshing/Mesh.cs @@ -259,6 +259,7 @@ namespace OpenSim.Region.Physics.Meshing public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount) { // A vertex is 3 floats + vertexStride = 3 * sizeof(float); // If there isn't an unmanaged array allocated yet, do it now diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index b462713..fd4ac7f 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -702,10 +702,11 @@ namespace OpenSim.Region.Physics.Meshing return CreateMesh(primName, primShape, size, lod, false); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) { return CreateMesh(primName, primShape, size, lod, false); } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { #if SPAM diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index c31ec08..a0a18c4 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -315,6 +315,32 @@ namespace OpenSim.Region.Physics.Meshing return result; } + public void PrepForOde() + { + // If there isn't an unmanaged array allocated yet, do it now + if (m_verticesPtr == IntPtr.Zero) + { + float[] vertexList = getVertexListAsFloat(); + // Each vertex is 3 elements (floats) + m_vertexCount = vertexList.Length / 3; + int byteCount = m_vertexCount * 3 * sizeof(float); + m_verticesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount); + System.Runtime.InteropServices.Marshal.Copy(vertexList, 0, m_verticesPtr, m_vertexCount * 3); + } + + // If there isn't an unmanaged array allocated yet, do it now + if (m_indicesPtr == IntPtr.Zero) + { + int[] indexList = getIndexListAsInt(); + m_indexCount = indexList.Length; + int byteCount = m_indexCount * sizeof(int); + m_indicesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount); + System.Runtime.InteropServices.Marshal.Copy(indexList, 0, m_indicesPtr, m_indexCount); + } + + releaseSourceMeshData(); + } + public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount) { // A vertex is 3 floats diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index fabadd3..dec5eb7 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -993,12 +993,12 @@ namespace OpenSim.Region.Physics.Meshing public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) { - return CreateMesh(primName, primShape, size, lod, false,false); + return CreateMesh(primName, primShape, size, lod, false,false,false); } public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { - return CreateMesh(primName, primShape, size, lod, false,false); + return CreateMesh(primName, primShape, size, lod, false,false,false); } private static Vector3 m_MeshUnitSize = new Vector3(0.5f, 0.5f, 0.5f); @@ -1039,7 +1039,7 @@ namespace OpenSim.Region.Physics.Meshing return null; } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) { #if SPAM m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName); @@ -1094,8 +1094,14 @@ namespace OpenSim.Region.Physics.Meshing mesh.DumpRaw(baseDir, primName, "Z extruded"); } - // trim the vertex and triangle lists to free up memory - mesh.TrimExcess(); + if (forOde) + { + // force pinned mem allocation + mesh.PrepForOde(); + } + else + mesh.TrimExcess(); + mesh.Key = key; mesh.RefCount = 1; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 024835c..24f76d62 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -518,7 +518,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex); + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex,true); } repData.mesh = mesh; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index fc59180..d993f22 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1384,7 +1384,6 @@ namespace OpenSim.Region.Physics.OdePlugin IntPtr vertices, indices; int vertexCount, indexCount; int vertexStride, triStride; - IMesh mesh = m_mesh; -- cgit v1.1 From d0773dcd6aa10375f16fb4f268bfcecf10056ac9 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 05:09:43 +0100 Subject: another debug msg --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index beaba13..229bf8e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -719,6 +719,28 @@ namespace OpenSim.Region.Physics.OdePlugin if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; +// debug + PhysicsActor dp2; + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + { + d.AABB aabb; + d.GeomGetAABB(g2, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if (x > 60.0f || y > 60.0f || z > 60.0f) + { + if (!actor_name_map.TryGetValue(g2, out dp2)) + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + else + m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5}", + dp2.Name, dp2.Size, x, y, z, dp2.Position); + } + } +// + + + if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) { -- cgit v1.1 From 315f3ee0e59a894271fb7586cbb85f7859df75f3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 05:21:09 +0100 Subject: avoid crashing so debug is seen --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 1 + 1 file changed, 1 insertion(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 229bf8e..fe6105f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -735,6 +735,7 @@ namespace OpenSim.Region.Physics.OdePlugin else m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5}", dp2.Name, dp2.Size, x, y, z, dp2.Position); + return; } } // -- cgit v1.1 From ce497ce379e2b741727456dc3e4c99b68302f135 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 19:43:06 +0100 Subject: debug... --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index fe6105f..03048a4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -733,8 +733,11 @@ namespace OpenSim.Region.Physics.OdePlugin if (!actor_name_map.TryGetValue(g2, out dp2)) m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); else - m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5}", - dp2.Name, dp2.Size, x, y, z, dp2.Position); + m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", + dp2.Name, dp2.Size, x, y, z, + dp2.Position.ToString(), + dp2.Orientation.ToString(), + dp2.Orientation.Length()); return; } } -- cgit v1.1 From e238ece327cb24a7f40b39a04e22e0da14db2960 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 22:57:28 +0100 Subject: debug... --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index d993f22..0189e42 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -641,6 +641,11 @@ namespace OpenSim.Region.Physics.OdePlugin { fakeori = value; givefakeori++; +// Debug + float qlen = value.Length(); + if (value.Length() > 1.01f || qlen <0.99) + m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion Orientation from Scene in Object {0} norm {}", Name, qlen); +// AddChange(changes.Orientation, value); } else -- cgit v1.1 From a19a189fec6afa44c632ec83c97d649efc832217 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 9 Oct 2012 23:01:26 +0100 Subject: fix debug :) --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 0189e42..0400899 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -644,7 +644,7 @@ namespace OpenSim.Region.Physics.OdePlugin // Debug float qlen = value.Length(); if (value.Length() > 1.01f || qlen <0.99) - m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion Orientation from Scene in Object {0} norm {}", Name, qlen); + m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion Orientation from Scene in Object {0} norm {1}", Name, qlen); // AddChange(changes.Orientation, value); } -- cgit v1.1 From c0cdeec4c01624e5cbeda657b2edd111f75b5954 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 10 Oct 2012 00:36:06 +0100 Subject: debug --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 0400899..e086264 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2247,6 +2247,12 @@ namespace OpenSim.Region.Physics.OdePlugin _orientation.Z = qtmp.Z; _orientation.W = qtmp.W; +// Debug + float qlen = _orientation.Length(); + if (qlen > 1.01f || qlen < 0.99) + m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); +// + d.Vector3 lpos = d.GeomGetPosition(prim_geom); _position.X = lpos.X; _position.Y = lpos.Y; -- cgit v1.1 From 8fa91686dbee7071d2a718886cdc11e6751ccbff Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 10 Oct 2012 00:57:33 +0100 Subject: add some quaternion normalizations to keep errors under control --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 3 +++ 1 file changed, 3 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index e086264..eaee950 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -646,6 +646,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.Length() > 1.01f || qlen <0.99) m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion Orientation from Scene in Object {0} norm {1}", Name, qlen); // + value.Normalize(); + AddChange(changes.Orientation, value); } else @@ -2252,6 +2254,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (qlen > 1.01f || qlen < 0.99) m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); // + _orientation.Normalize(); d.Vector3 lpos = d.GeomGetPosition(prim_geom); _position.X = lpos.X; -- cgit v1.1 From d5cfe1c0be89f5d212b77c4b0e750ef409ba09bd Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 12 Oct 2012 00:36:01 +0100 Subject: remove some more debug spam on ode --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 5388 ++++++++++------------ OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 5209 +++++++++++++-------- 2 files changed, 5783 insertions(+), 4814 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index eaee950..eaf0d0a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -25,11 +25,6 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -/* Revision 2011/12 by Ubit Umarov - * - * - */ - /* * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: @@ -53,251 +48,250 @@ using System.Runtime.InteropServices; using System.Threading; using log4net; using OpenMetaverse; -using OdeAPI; +using Ode.NET; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; namespace OpenSim.Region.Physics.OdePlugin { + /// + /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. + /// public class OdePrim : PhysicsActor { private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); private bool m_isphysical; - private bool m_fakeisphysical; - private bool m_isphantom; - private bool m_fakeisphantom; - internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - protected bool m_building; - protected bool m_forcePosOrRotation; - private bool m_iscolliding; + public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } } + private int m_expectedCollisionContacts = 0; - internal bool m_isSelected; - private bool m_delaySelect; - private bool m_lastdoneSelected; - internal bool m_outbounds; - - private Quaternion m_lastorientation; - private Quaternion _orientation; + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical + { + get { return m_isphysical; } + set + { + m_isphysical = value; + if (!m_isphysical) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + } + } private Vector3 _position; private Vector3 _velocity; private Vector3 _torque; private Vector3 m_lastVelocity; private Vector3 m_lastposition; + private Quaternion m_lastorientation = new Quaternion(); private Vector3 m_rotationalVelocity; private Vector3 _size; private Vector3 _acceleration; + // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); + private Quaternion _orientation; + private Vector3 m_taintposition; + private Vector3 m_taintsize; + private Vector3 m_taintVelocity; + private Vector3 m_taintTorque; + private Quaternion m_taintrot; private Vector3 m_angularlock = Vector3.One; - private IntPtr Amotor; + private Vector3 m_taintAngularLock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; - private Vector3 m_force; - private Vector3 m_forceacc; - private Vector3 m_angularForceacc; - - private float m_invTimeStep; - private float m_timeStep; + private object m_assetsLock = new object(); + private bool m_assetFailed = false; private Vector3 m_PIDTarget; private float m_PIDTau; + private float PID_D = 35f; + private float PID_G = 25f; private bool m_usePID; + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + private float m_PIDHoverHeight; private float m_PIDHoverTau; private bool m_useHoverPID; - private PIDHoverType m_PIDHoverType; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; private float m_targetHoverHeight; private float m_groundHeight; private float m_waterHeight; private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - private int body_autodisable_frames; - public int bodydisablecontrol; - + // private float m_tensor = 5f; + private int body_autodisable_frames = 20; - // Default we're a Geometry - private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); - // Default colide nonphysical don't try to colide with anything - private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; - private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | - CollisionCategories.Character | - CollisionCategories.Land | - CollisionCategories.VolumeDtc); - -// private bool m_collidesLand = true; + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + private bool m_taintshape; + private bool m_taintPhysics; + private bool m_collidesLand = true; private bool m_collidesWater; -// public bool m_returnCollisions; - - private bool m_NoColide; // for now only for internal use for bad meshs + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); // Default, Collide with Other Geometries, spaces and Bodies - private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; + private CollisionCategories m_collisionFlags = m_default_collisionFlags; - public bool m_disabled; + public bool m_taintremove { get; private set; } + public bool m_taintdisable { get; private set; } + internal bool m_disabled; + public bool m_taintadd { get; private set; } + public bool m_taintselected { get; private set; } + public bool m_taintCollidesWater { get; private set; } - private uint m_localID; + private bool m_taintforce = false; + private bool m_taintaddangularforce = false; + private Vector3 m_force; + private List m_forcelist = new List(); + private List m_angularforcelist = new List(); - private IMesh m_mesh; - private object m_meshlock = new object(); private PrimitiveBaseShape _pbs; + private OdeScene _parent_scene; - private UUID? m_assetID; - private MeshState m_meshState; - - public OdeScene _parent_scene; + /// + /// The physics space which contains prim geometries + /// + public IntPtr m_targetSpace = IntPtr.Zero; /// - /// The physics space which contains prim geometry + /// The prim geometry, used for collision detection. /// - public IntPtr m_targetSpace; + /// + /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or + /// mesh change) or when the physical prim is being removed from the scene. + /// + public IntPtr prim_geom { get; private set; } - public IntPtr prim_geom; - public IntPtr _triMeshData; + public IntPtr _triMeshData { get; private set; } + private IntPtr _linkJointGroup = IntPtr.Zero; private PhysicsActor _parent; + private PhysicsActor m_taintparent; private List childrenPrim = new List(); - public float m_collisionscore; - private int m_colliderfilter = 0; - - public IntPtr collide_geom; // for objects: geom if single prim space it linkset - - private float m_density; - private byte m_shapetype; - public bool _zeroFlag; - private bool m_lastUpdateSent; + private bool iscolliding; + private bool m_isSelected; - public IntPtr Body; + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - private Vector3 _target_velocity; + private bool m_throttleUpdates; + private int throttleCounter; + public int m_interpenetrationcount { get; private set; } + internal float m_collisionscore; + public int m_roundsUnderMotionThreshold { get; private set; } + private int m_crossingfailures; - public Vector3 m_OBBOffset; - public Vector3 m_OBB; - public float primOOBradiusSQ; + public bool outofBounds { get; private set; } + private float m_density = 10.000006836f; // Aluminum g/cm3; - private bool m_hasOBB = true; + public bool _zeroFlag { get; private set; } + private bool m_lastUpdateSent; - private float m_physCost; - private float m_streamCost; + public IntPtr Body = IntPtr.Zero; + private Vector3 _target_velocity; + private d.Mass pMass; - public d.Mass primdMass; // prim inertia information on it's own referencial - float primMass; // prim own mass - float primVolume; // prim own volume; - float _mass; // object mass acording to case + private int m_eventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); - public int givefakepos; - private Vector3 fakepos; - public int givefakeori; - private Quaternion fakeori; + /// + /// Signal whether there were collisions on the previous frame, so we know if we need to send the + /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision. + /// + /// + /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself. + /// + private bool m_collisionsOnPreviousFrame; - private int m_eventsubscription; - private int m_cureventsubscription; - private CollisionEventUpdate CollisionEventsThisFrame = null; - private bool SentEmptyCollisionsEvent; + private IntPtr m_linkJoint = IntPtr.Zero; - public volatile bool childPrim; + internal volatile bool childPrim; - public ODEDynamics m_vehicle; + private ODEDynamics m_vehicle; internal int m_material = (int)Material.Wood; - private float mu; - private float bounce; - /// - /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. - /// - public override bool IsPhysical // this is not reliable for internal use + public OdePrim( + String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) { - get { return m_fakeisphysical; } - set - { - m_fakeisphysical = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - - if (!value) // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - AddChange(changes.Physical, value); - } - } + Name = primName; + m_vehicle = new ODEDynamics(); + //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); - public override bool IsVolumeDtc - { - get { return m_fakeisVolumeDetect; } - set + if (!pos.IsFinite()) { - m_fakeisVolumeDetect = value; - AddChange(changes.VolumeDtc, value); + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); } - } + _position = pos; + m_taintposition = pos; + PID_D = parent_scene.bodyPIDD; + PID_G = parent_scene.bodyPIDG; + m_density = parent_scene.geomDefaultDensity; + // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - public override bool Phantom // this is not reliable for internal use - { - get { return m_fakeisphantom; } - set - { - m_fakeisphantom = value; - AddChange(changes.Phantom, value); - } - } + prim_geom = IntPtr.Zero; - public override bool Building // this is not reliable for internal use - { - get { return m_building; } - set + if (!pos.IsFinite()) { - if (value) - m_building = true; - AddChange(changes.building, value); + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); } - } - public override void getContactData(ref ContactData cdata) - { - cdata.mu = mu; - cdata.bounce = bounce; + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; - // cdata.softcolide = m_softcolide; - cdata.softcolide = false; + _size = size; + m_taintsize = _size; - if (m_isphysical) + if (!QuaternionIsFinite(rotation)) { - ODEDynamics veh; - if (_parent != null) - veh = ((OdePrim)_parent).m_vehicle; - else - veh = m_vehicle; - - if (veh != null && veh.Type != Vehicle.TYPE_NONE) - cdata.mu *= veh.FrictionFactor; -// cdata.mu *= 0; + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); } - } - public override float PhysicsCost - { - get + _orientation = rotation; + m_taintrot = _orientation; + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = (IntPtr)0; + + if (pos.Z < 0) { - return m_physCost; + IsPhysical = false; } - } - - public override float StreamCost - { - get + else { - return m_streamCost; + IsPhysical = pisPhysical; + // If we're physical, we need to be in the master space for now. + // linksets *should* be in a space together.. but are not currently + if (IsPhysical) + m_targetSpace = _parent_scene.space; } + + m_taintadd = true; + m_assetFailed = false; + _parent_scene.AddPhysicsActorTaint(this); } public override int PhysicsActorType { - get { return (int)ActorTypes.Prim; } + get { return (int) ActorTypes.Prim; } set { return; } } @@ -307,23 +301,6 @@ namespace OpenSim.Region.Physics.OdePlugin set { return; } } - public override uint LocalID - { - get { return m_localID; } - set { m_localID = value; } - } - - public override PhysicsActor ParentActor - { - get - { - if (childPrim) - return _parent; - else - return (PhysicsActor)this; - } - } - public override bool Grabbed { set { return; } @@ -333,3068 +310,2383 @@ namespace OpenSim.Region.Physics.OdePlugin { set { - if (value) - m_isSelected = value; // if true set imediatly to stop moves etc - AddChange(changes.Selected, value); - } - } - - public override bool Flying - { - // no flying prims for you - get { return false; } - set { } - } + // This only makes the object not collidable if the object + // is physical or the object is modified somehow *IN THE FUTURE* + // without this, if an avatar selects prim, they can walk right + // through it while it's selected + m_collisionscore = 0; - public override bool IsColliding - { - get { return m_iscolliding; } - set - { - if (value) + if ((IsPhysical && !_zeroFlag) || !value) { - m_colliderfilter += 2; - if (m_colliderfilter > 2) - m_colliderfilter = 2; + m_taintselected = value; + _parent_scene.AddPhysicsActorTaint(this); } else { - m_colliderfilter--; - if (m_colliderfilter < 0) - m_colliderfilter = 0; + m_taintselected = value; + m_isSelected = value; } - if (m_colliderfilter == 0) - m_iscolliding = false; - else - m_iscolliding = true; + if (m_isSelected) + disableBodySoft(); } } - public override bool CollidingGround - { - get { return false; } - set { return; } - } - - public override bool CollidingObj - { - get { return false; } - set { return; } - } - - - public override bool ThrottleUpdates {get;set;} - - public override bool Stopped + /// + /// Set a new geometry for this prim. + /// + /// + private void SetGeom(IntPtr geom) { - get { return _zeroFlag; } - } + prim_geom = geom; +//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); - public override Vector3 Position - { - get - { - if (givefakepos > 0) - return fakepos; - else - return _position; - } + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - set - { - fakepos = value; - givefakepos++; - AddChange(changes.Position, value); - } - } + _parent_scene.geom_name_map[prim_geom] = Name; + _parent_scene.actor_name_map[prim_geom] = this; - public override Vector3 Size - { - get { return _size; } - set + if (childPrim) { - if (value.IsFinite()) - { - _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype); - } - else + if (_parent != null && _parent is OdePrim) { - m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + OdePrim parent = (OdePrim)_parent; +//Console.WriteLine("SetGeom calls ChildSetGeom"); + parent.ChildSetGeom(this); } } + //m_log.Warn("Setting Geom to: " + prim_geom); } - public override float Mass - { - get { return primMass; } - } - - public override Vector3 Force + private void enableBodySoft() { - get { return m_force; } - set + if (!childPrim) { - if (value.IsFinite()) - { - AddChange(changes.Force, value); - } - else + if (IsPhysical && Body != IntPtr.Zero) { - m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + d.BodyEnable(Body); + if (m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Enable(Body, _parent_scene); } + + m_disabled = false; } } - public override void SetVolumeDetect(int param) + private void disableBodySoft() { - m_fakeisVolumeDetect = (param != 0); - AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); - } + m_disabled = true; - public override Vector3 GeometricCenter - { - // this is not real geometric center but a average of positions relative to root prim acording to - // http://wiki.secondlife.com/wiki/llGetGeometricCenter - // ignoring tortured prims details since sl also seems to ignore - // so no real use in doing it on physics - get + if (IsPhysical && Body != IntPtr.Zero) { - return Vector3.Zero; + d.BodyDisable(Body); } } - public override Vector3 CenterOfMass + /// + /// Make a prim subject to physics. + /// + private void enableBody() { - get + // Don't enable this body if we're a child prim + // this should be taken care of in the parent function not here + if (!childPrim) { - lock (_parent_scene.OdeLock) - { - d.Vector3 dtmp; - if (!childPrim && Body != IntPtr.Zero) - { - dtmp = d.BodyGetPosition(Body); - return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - } - else if (prim_geom != IntPtr.Zero) - { - d.Quaternion dq; - d.GeomCopyQuaternion(prim_geom, out dq); - Quaternion q; - q.X = dq.X; - q.Y = dq.Y; - q.Z = dq.Z; - q.W = dq.W; - - Vector3 Ptot = m_OBBOffset * q; - dtmp = d.GeomGetPosition(prim_geom); - Ptot.X += dtmp.X; - Ptot.Y += dtmp.Y; - Ptot.Z += dtmp.Z; - - // if(childPrim) we only know about physical linksets - return Ptot; -/* - float tmass = _mass; - Ptot *= tmass; + // Sets the geom to a body + Body = d.BodyCreate(_parent_scene.world); - float m; - - foreach (OdePrim prm in childrenPrim) - { - m = prm._mass; - Ptot += prm.CenterOfMass * m; - tmass += m; - } + setMass(); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.BodySetQuaternion(Body, ref myrot); + d.GeomSetBody(prim_geom, Body); + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - if (tmass == 0) - tmass = 0; - else - tmass = 1.0f / tmass; + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - Ptot *= tmass; - return Ptot; -*/ - } - else - return _position; - } - } - } + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode (Body, false); - public override Vector3 OOBsize - { - get - { - return m_OBB; - } - } + m_interpenetrationcount = 0; + m_collisionscore = 0; + m_disabled = false; - public override Vector3 OOBoffset - { - get + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) { - return m_OBBOffset; + createAMotor(m_angularlock); } - } - - public override float OOBRadiusSQ - { - get + if (m_vehicle.Type != Vehicle.TYPE_NONE) { - return primOOBradiusSQ; + m_vehicle.Enable(Body, _parent_scene); } - } - public override PrimitiveBaseShape Shape - { - set - { -// AddChange(changes.Shape, value); - _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype); + _parent_scene.ActivatePrim(this); } } - public override byte PhysicsShapeType - { - get - { - return m_shapetype; - } - set - { - m_shapetype = value; - _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); - } - } + #region Mass Calculation - public override Vector3 Velocity + private float CalculateMass() { - get - { - if (_zeroFlag) - return Vector3.Zero; - return _velocity; - } - set + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float returnMass = 0; + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) { - if (value.IsFinite()) - { - AddChange(changes.Velocity, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); - } + case ProfileShape.Square: + // default box - } - } - - public override Vector3 Torque - { - get - { - if (!IsPhysical || Body == IntPtr.Zero) - return Vector3.Zero; - - return _torque; - } - - set - { - if (value.IsFinite()) - { - AddChange(changes.Torque, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); - } - } - } - - public override float CollisionScore - { - get { return m_collisionscore; } - set { m_collisionscore = value; } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override Quaternion Orientation - { - get - { - if (givefakeori > 0) - return fakeori; - else - - return _orientation; - } - set - { - if (QuaternionIsFinite(value)) - { - fakeori = value; - givefakeori++; -// Debug - float qlen = value.Length(); - if (value.Length() > 1.01f || qlen <0.99) - m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion Orientation from Scene in Object {0} norm {1}", Name, qlen); -// - value.Normalize(); - - AddChange(changes.Orientation, value); - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); - - } - } - - public override Vector3 Acceleration - { - get { return _acceleration; } - set { } - } - - public override Vector3 RotationalVelocity - { - get - { - Vector3 pv = Vector3.Zero; - if (_zeroFlag) - return pv; - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - return pv; - - return m_rotationalVelocity; - } - set - { - if (value.IsFinite()) - { - AddChange(changes.AngVelocity, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); - } - } - } - - public override float Buoyancy - { - get { return m_buoyancy; } - set - { - AddChange(changes.Buoyancy,value); - } - } - - public override bool FloatOnWater - { - set - { - AddChange(changes.CollidesWater, value); - } - } - - public override Vector3 PIDTarget - { - set - { - if (value.IsFinite()) - { - AddChange(changes.PIDTarget,value); - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); - } - } - - public override bool PIDActive - { - set - { - AddChange(changes.PIDActive,value); - } - } - - public override float PIDTau - { - set - { - float tmp = 0; - if (value > 0) - { - float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); - if (value < mint) - tmp = mint; - else - tmp = value; - } - AddChange(changes.PIDTau,tmp); - } - } - - public override float PIDHoverHeight - { - set - { - AddChange(changes.PIDHoverHeight,value); - } - } - public override bool PIDHoverActive - { - set - { - AddChange(changes.PIDHoverActive, value); - } - } - - public override PIDHoverType PIDHoverType - { - set - { - AddChange(changes.PIDHoverType,value); - } - } - - public override float PIDHoverTau - { - set - { - float tmp =0; - if (value > 0) - { - float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); - if (value < mint) - tmp = mint; - else - tmp = value; - } - AddChange(changes.PIDHoverTau, tmp); - } - } - - public override Quaternion APIDTarget { set { return; } } - - public override bool APIDActive { set { return; } } - - public override float APIDStrength { set { return; } } - - public override float APIDDamping { set { return; } } - - public override int VehicleType - { - // we may need to put a fake on this - get - { - if (m_vehicle == null) - return (int)Vehicle.TYPE_NONE; - else - return (int)m_vehicle.Type; - } - set - { - AddChange(changes.VehicleType, value); - } - } - - public override void VehicleFloatParam(int param, float value) - { - strVehicleFloatParam fp = new strVehicleFloatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleFloatParam, fp); - } + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; - public override void VehicleVectorParam(int param, Vector3 value) - { - strVehicleVectorParam fp = new strVehicleVectorParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleVectorParam, fp); - } + case HollowShape.Circle: - public override void VehicleRotationParam(int param, Quaternion value) - { - strVehicleQuatParam fp = new strVehicleQuatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleRotationParam, fp); - } + hollowVolume *= 0.78539816339f; + break; - public override void VehicleFlags(int param, bool value) - { - strVehicleBoolParam bp = new strVehicleBoolParam(); - bp.param = param; - bp.value = value; - AddChange(changes.VehicleFlags, bp); - } + case HollowShape.Triangle: - public override void SetVehicle(object vdata) - { - AddChange(changes.SetVehicle, vdata); - } - public void SetAcceleration(Vector3 accel) - { - _acceleration = accel; - } + hollowVolume *= (0.5f * .5f); + break; - public override void AddForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - if(pushforce) - AddChange(changes.AddForce, force); - else // a impulse - AddChange(changes.AddForce, force * m_invTimeStep); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); - } - //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); - } - - public override void AddAngularForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { -// if(pushforce) for now applyrotationimpulse seems more happy applied as a force - AddChange(changes.AddAngForce, force); -// else // a impulse -// AddChange(changes.AddAngForce, force * m_invTimeStep); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); - } - } - - public override void CrossingFailure() - { - if (m_outbounds) - { - _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); - _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); - _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); - - m_lastposition = _position; - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } - m_lastVelocity = _velocity; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube - if(Body != IntPtr.Zero) - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - if (prim_geom != IntPtr.Zero) - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume*tmp*tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f;; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } - m_outbounds = false; - changeDisable(false); - base.RequestPhysicsterseUpdate(); - } - } + break; - public override void SetMomentum(Vector3 momentum) - { - } + case ProfileShape.Circle: - public override void SetMaterial(int pMaterial) - { - m_material = pMaterial; - mu = _parent_scene.m_materialContactsData[pMaterial].mu; - bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; - } + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base - public void setPrimForRemoval() - { - AddChange(changes.Remove, null); - } + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; - public override void link(PhysicsActor obj) - { - AddChange(changes.Link, obj); - } + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; - public override void delink() - { - AddChange(changes.DeLink, null); - } + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; - public override void LockAngularMotion(Vector3 axis) - { - // reverse the zero/non zero values for ODE. - if (axis.IsFinite()) - { - axis.X = (axis.X > 0) ? 1f : 0f; - axis.Y = (axis.Y > 0) ? 1f : 0f; - axis.Z = (axis.Z > 0) ? 1f : 0f; -// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - AddChange(changes.AngLock, axis); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); - } - } + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - m_cureventsubscription = 0; - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); - SentEmptyCollisionsEvent = false; - } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { - public override void UnSubscribeEvents() - { - if (CollisionEventsThisFrame != null) - { - CollisionEventsThisFrame.Clear(); - CollisionEventsThisFrame = null; - } - m_eventsubscription = 0; - _parent_scene.RemoveCollisionEventReporting(this); - } + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; - public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) - { - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); -// if(CollisionEventsThisFrame.Count < 32) - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - } + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; - public void SendCollisions() - { - if (CollisionEventsThisFrame == null) - return; + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; - if (m_cureventsubscription < m_eventsubscription) - return; + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; - m_cureventsubscription = 0; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; - int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.52359877559829887307710723054658f; + } + break; - if (!SentEmptyCollisionsEvent || ncolisions > 0) - { - base.SendCollisionUpdate(CollisionEventsThisFrame); + case ProfileShape.EquilateralTriangle: - if (ncolisions == 0) - { - SentEmptyCollisionsEvent = true; - _parent_scene.RemoveCollisionEventReporting(this); - } - else - { - SentEmptyCollisionsEvent = false; - CollisionEventsThisFrame.Clear(); - } - } - } + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; - internal void AddCollisionFrameTime(int t) - { - if (m_cureventsubscription < 50000) - m_cureventsubscription += t; - } + if (hollowAmount > 0.0) + { - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; - public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) - { - Name = primName; - LocalID = plocalID; + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; - m_vehicle = null; + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation - if (!pos.IsFinite()) - { - pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), - parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); - } - _position = pos; - givefakepos = 0; + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; - m_timeStep = parent_scene.ODE_STEPSIZE; - m_invTimeStep = 1f / m_timeStep; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); - m_density = parent_scene.geomDefaultDensity; - body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + if (hollowAmount > 0.0) + { - prim_geom = IntPtr.Zero; - collide_geom = IntPtr.Zero; - Body = IntPtr.Zero; + hollowVolume *= hollowAmount; - if (!size.IsFinite()) - { - size = new Vector3(0.5f, 0.5f, 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); - } + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; - if (size.X <= 0) size.X = 0.01f; - if (size.Y <= 0) size.Y = 0.01f; - if (size.Z <= 0) size.Z = 0.01f; + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; - _size = size; + case HollowShape.Circle: - if (!QuaternionIsFinite(rotation)) - { - rotation = Quaternion.Identity; - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); - } + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; - _orientation = rotation; - givefakeori = 0; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; - _pbs = pbs; + default: + break; + } - _parent_scene = parent_scene; - m_targetSpace = IntPtr.Zero; + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; - if (pos.Z < 0) + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) { - m_isphysical = false; + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; } else { - m_isphysical = pisPhysical; + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; } - m_fakeisphysical = m_isphysical; - m_isVolumeDetect = false; - m_fakeisVolumeDetect = false; + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); - m_force = Vector3.Zero; + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); - m_iscolliding = false; - m_colliderfilter = 0; - m_NoColide = false; +// this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); - _triMeshData = IntPtr.Zero; + returnMass = m_density * volume; - m_shapetype = _shapeType; + if (returnMass <= 0) + returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. +// else if (returnMass > _parent_scene.maximumMassObject) +// returnMass = _parent_scene.maximumMassObject; - m_lastdoneSelected = false; - m_isSelected = false; - m_delaySelect = false; + // Recursively calculate mass + bool HasChildPrim = false; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + HasChildPrim = true; + } + } - m_isphantom = pisPhantom; - m_fakeisphantom = pisPhantom; + if (HasChildPrim) + { + OdePrim[] childPrimArr = new OdePrim[0]; - mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; - bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + lock (childrenPrim) + childPrimArr = childrenPrim.ToArray(); - m_building = true; // control must set this to false when done + for (int i = 0; i < childPrimArr.Length; i++) + { + if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) + returnMass += childPrimArr[i].CalculateMass(); + // failsafe, this shouldn't happen but with OpenSim, you never know :) + if (i > 256) + break; + } + } - _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); - } + if (returnMass > _parent_scene.maximumMassObject) + returnMass = _parent_scene.maximumMassObject; - private void resetCollisionAccounting() - { - m_collisionscore = 0; + return returnMass; } - private void UpdateCollisionCatFlags() + #endregion + + private void setMass() { - if(m_isphysical && m_disabled) + if (Body != (IntPtr) 0) { - m_collisionCategories = 0; - m_collisionFlags = 0; - } + float newmass = CalculateMass(); - else if (m_isSelected) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = 0; - } + //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString()); - else if (m_isVolumeDetect) - { - m_collisionCategories = CollisionCategories.VolumeDtc; - if (m_isphysical) - m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; - else - m_collisionFlags = 0; - } - else if (m_isphantom) - { - m_collisionCategories = CollisionCategories.Phantom; - if (m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; - } - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionFlags = m_default_collisionFlagsPhysical; - else - m_collisionFlags = m_default_collisionFlagsNotPhysical; + d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z); + d.BodySetMass(Body, ref pMass); } } - private void ApplyCollisionCatFlags() + /// + /// Stop a prim from being subject to physics. + /// + internal void disableBody() { - if (prim_geom != IntPtr.Zero) + //this kills the body so things like 'mesh' can re-create it. + lock (this) { - if (!childPrim && childrenPrim.Count > 0) + if (!childPrim) { - foreach (OdePrim prm in childrenPrim) + if (Body != IntPtr.Zero) { - if (m_isphysical && m_disabled) - { - prm.m_collisionCategories = 0; - prm.m_collisionFlags = 0; - } - else - { - // preserve some - if (prm.m_isSelected) - { - prm.m_collisionCategories = CollisionCategories.Selected; - prm.m_collisionFlags = 0; - } - else if (prm.m_isVolumeDetect) - { - prm.m_collisionCategories = CollisionCategories.VolumeDtc; - if (m_isphysical) - prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; - else - prm.m_collisionFlags = 0; - } - else if (prm.m_isphantom) - { - prm.m_collisionCategories = CollisionCategories.Phantom; - if (m_isphysical) - prm.m_collisionFlags = CollisionCategories.Land; - else - prm.m_collisionFlags = 0; - } - else - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - } - } + _parent_scene.DeactivatePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - if (prm.prim_geom != IntPtr.Zero) + d.BodyDestroy(Body); + lock (childrenPrim) { - if (prm.m_NoColide) + if (childrenPrim.Count > 0) { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.DeactivatePrim(prm); + prm.Body = IntPtr.Zero; + } } } - } - } - - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); - if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(collide_geom, 0); - d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); + Body = IntPtr.Zero; } } else { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); - if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); - } - } - } - } + _parent_scene.DeactivatePrim(this); + + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - private void createAMotor(Vector3 axis) - { - if (Body == IntPtr.Zero) - return; + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; + Body = IntPtr.Zero; + } } - int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); - - if (axisnum <= 0) - return; - - // stop it - d.BodySetTorque(Body, 0, 0, 0); - d.BodySetAngularVel(Body, 0, 0, 0); - - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - - d.JointSetAMotorMode(Amotor, 0); - - d.JointSetAMotorNumAxes(Amotor, axisnum); - - // get current orientation to lock - - d.Quaternion dcur = d.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; - Vector3 ax; + m_disabled = true; + m_collisionscore = 0; + } - int i = 0; - int j = 0; - if (axis.X == 0) - { - ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X - // ODE should do this with axis relative to body 1 but seems to fail - d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, 0, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); - i++; - j = 256; // move to next axis set - } + private static Dictionary m_MeshToTriMeshMap = new Dictionary(); - if (axis.Y == 0) - { - ax = (new Vector3(0, 1, 0)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - i++; - j += 256; - } + private void setMesh(OdeScene parent_scene, IMesh mesh) + { +// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh); - if (axis.Z == 0) - { - ax = (new Vector3(0, 0, 1)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - } - } + // This sleeper is there to moderate how long it takes between + // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object + //Thread.Sleep(10); - private void SetGeom(IntPtr geom) - { - prim_geom = geom; - //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); - if (prim_geom != IntPtr.Zero) + //Kill Body so that mesh can re-make the geom + if (IsPhysical && Body != IntPtr.Zero) { - - if (m_NoColide) + if (childPrim) { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - { - d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); - } - else + if (_parent != null) { - d.GeomSetCollideBits(prim_geom, 0); - d.GeomDisable(prim_geom); + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); } } else { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + disableBody(); } - - UpdatePrimBodyData(); - _parent_scene.actor_name_map[prim_geom] = this; - - -// debug - d.AABB aabb; - d.GeomGetAABB(prim_geom, out aabb); - float x = aabb.MaxX - aabb.MinX; - float y = aabb.MaxY - aabb.MinY; - float z = aabb.MaxZ - aabb.MinZ; - if( x > 60.0f || y > 60.0f || z > 60.0f) - m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", - Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); - else if (x < 0.001f || y < 0.001f || z < 0.001f) - m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", - Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); - -// - - - - } - else - m_log.Warn("Setting bad Geom"); - } - private bool GetMeshGeom() - { IntPtr vertices, indices; int vertexCount, indexCount; int vertexStride, triStride; - - IMesh mesh = m_mesh; - - if (mesh == null) - return false; - - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + m_expectedCollisionContacts = indexCount; + mesh.releaseSourceMeshData(); // free up the original mesh data to save memory - if (vertexCount == 0 || indexCount == 0) + // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at + // the same time. + lock (m_MeshToTriMeshMap) { - m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1}", - Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh"); - - m_hasOBB = false; - m_OBBOffset = Vector3.Zero; - m_OBB = _size * 0.5f; - - m_physCost = 0.1f; - m_streamCost = 1.0f; - - _parent_scene.mesher.ReleaseMesh(mesh); - m_meshState = MeshState.MeshFailed; - m_mesh = null; - return false; + if (m_MeshToTriMeshMap.ContainsKey(mesh)) + { + _triMeshData = m_MeshToTriMeshMap[mesh]; + } + else + { + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + m_MeshToTriMeshMap[mesh] = _triMeshData; + } } - IntPtr geo = IntPtr.Zero; - +// _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - _triMeshData = d.GeomTriMeshDataCreate(); - - d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); - d.GeomTriMeshDataPreprocess(_triMeshData); - - geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); } - - catch (Exception e) + catch (AccessViolationException) { - m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); - if (_triMeshData != IntPtr.Zero) - { - try - { - d.GeomTriMeshDataDestroy(_triMeshData); - } - catch - { - } - } - _triMeshData = IntPtr.Zero; - - m_hasOBB = false; - m_OBBOffset = Vector3.Zero; - m_OBB = _size * 0.5f; - m_physCost = 0.1f; - m_streamCost = 1.0f; - - _parent_scene.mesher.ReleaseMesh(mesh); - m_meshState = MeshState.MeshFailed; - m_mesh = null; - return false; + m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name); + return; } - m_physCost = 0.0013f * (float)indexCount; - // todo - m_streamCost = 1.0f; + // if (IsPhysical && Body == (IntPtr) 0) + // { + // Recreate the body + // m_interpenetrationcount = 0; + // m_collisionscore = 0; - SetGeom(geo); - - return true; + // enableBody(); + // } } - private void CreateGeom() + internal void ProcessTaints() { - bool hasMesh = false; +#if SPAM +Console.WriteLine("ZProcessTaints for " + Name); +#endif - m_NoColide = false; + // This must be processed as the very first taint so that later operations have a prim_geom to work with + // if this is a new prim. + if (m_taintadd) + changeadd(); - if ((m_meshState & MeshState.FailMask) != 0) - m_NoColide = true; + if (!_position.ApproxEquals(m_taintposition, 0f)) + changemove(); - else if(m_mesh != null) + if (m_taintrot != _orientation) { - if (GetMeshGeom()) - hasMesh = true; + if (childPrim && IsPhysical) // For physical child prim... + { + rotate(); + // KF: ODE will also rotate the parent prim! + // so rotate the root back to where it was + OdePrim parent = (OdePrim)_parent; + parent.rotate(); + } else - m_NoColide = true; + { + //Just rotate the prim + rotate(); + } } + + if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent)) + changePhysicsStatus(); + if (!_size.ApproxEquals(m_taintsize, 0f)) + changesize(); - if (!hasMesh) - { - IntPtr geo = IntPtr.Zero; + if (m_taintshape) + changeshape(); - if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 - && _size.X == _size.Y && _size.Y == _size.Z) - { // it's a sphere - try - { - geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); - return; - } - } - else - {// do it as a box - try - { - geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); - } - catch (Exception e) - { - m_log.Warn("[PHYSICS]: Create box failed: {0}", e); - return; - } - } - m_physCost = 0.1f; - m_streamCost = 1.0f; - SetGeom(geo); - } - } + if (m_taintforce) + changeAddForce(); - private void RemoveGeom() - { - if (prim_geom != IntPtr.Zero) - { - _parent_scene.actor_name_map.Remove(prim_geom); + if (m_taintaddangularforce) + changeAddAngularForce(); - try - { - d.GeomDestroy(prim_geom); - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - } - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); - } + if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f)) + changeSetTorque(); - prim_geom = IntPtr.Zero; - collide_geom = IntPtr.Zero; - m_targetSpace = IntPtr.Zero; - } - else - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); - } + if (m_taintdisable) + changedisable(); + + if (m_taintselected != m_isSelected) + changeSelectedStatus(); + + if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) + changevelocity(); - lock (m_meshlock) + if (m_taintparent != _parent) + changelink(); + + if (m_taintCollidesWater != m_collidesWater) + changefloatonwater(); + + if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) + changeAngularLock(); + } + + /// + /// Change prim in response to an angular lock taint. + /// + private void changeAngularLock() + { + // do we have a Physical object? + if (Body != IntPtr.Zero) { - if (m_mesh != null) + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) { - _parent_scene.mesher.ReleaseMesh(m_mesh); - m_mesh = null; + if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) + { + //d.BodySetFiniteRotationMode(Body, 0); + //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); + createAMotor(m_taintAngularLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } } } - Body = IntPtr.Zero; - m_hasOBB = false; - } - - //sets non physical prim m_targetSpace to right space in spaces grid for static prims - // should only be called for non physical prims unless they are becoming non physical - private void SetInStaticSpace(OdePrim prim) - { - IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); - prim.m_targetSpace = targetSpace; - collide_geom = IntPtr.Zero; + // Store this for later in case we get turned into a separate body + m_angularlock = m_taintAngularLock; } - public void enableBodySoft() + /// + /// Change prim in response to a link taint. + /// + private void changelink() { - m_disabled = false; - if (!childPrim && !m_isSelected) + // If the newly set parent is not null + // create link + if (_parent == null && m_taintparent != null) { - if (m_isphysical && Body != IntPtr.Zero) + if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) { - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); + OdePrim obj = (OdePrim)m_taintparent; + //obj.disableBody(); +//Console.WriteLine("changelink calls ParentPrim"); + obj.AddChildPrim(this); - d.BodyEnable(Body); + /* + if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body) + { + _linkJointGroup = d.JointGroupCreate(0); + m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); + d.JointAttach(m_linkJoint, obj.Body, Body); + d.JointSetFixed(m_linkJoint); + } + */ } } - resetCollisionAccounting(); - } - - private void disableBodySoft() - { - m_disabled = true; - if (!childPrim) + // If the newly set parent is null + // destroy link + else if (_parent != null && m_taintparent == null) { - if (m_isphysical && Body != IntPtr.Zero) +//Console.WriteLine(" changelink B"); + + if (_parent is OdePrim) { - if (m_isSelected) - m_collisionFlags = CollisionCategories.Selected; - else - m_collisionCategories = 0; - m_collisionFlags = 0; - ApplyCollisionCatFlags(); - d.BodyDisable(Body); + OdePrim obj = (OdePrim)_parent; + obj.ChildDelink(this); + childPrim = false; + //_parent = null; } + + /* + if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) + d.JointGroupDestroy(_linkJointGroup); + + _linkJointGroup = (IntPtr)0; + m_linkJoint = (IntPtr)0; + */ } + + _parent = m_taintparent; + m_taintPhysics = IsPhysical; } - private void MakeBody() + /// + /// Add a child prim to this parent prim. + /// + /// Child prim + private void AddChildPrim(OdePrim prim) { - if (!m_isphysical) // only physical get bodies - return; - - if (childPrim) // child prims don't get bodies; + if (LocalID == prim.LocalID) return; - if (m_building) - return; - - if (prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); - return; - } - - if (Body != IntPtr.Zero) + if (Body == IntPtr.Zero) { - DestroyBody(); - m_log.Warn("[PHYSICS]: MakeBody called having a body"); + Body = d.BodyCreate(_parent_scene.world); + setMass(); } - if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + lock (childrenPrim) { - d.GeomSetBody(prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); - } + if (childrenPrim.Contains(prim)) + return; - d.Matrix3 mymat = new d.Matrix3(); - d.Quaternion myrot = new d.Quaternion(); - d.Mass objdmass = new d.Mass { }; +// m_log.DebugFormat( +// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID); - Body = d.BodyCreate(_parent_scene.world); + childrenPrim.Add(prim); - objdmass = primdMass; + foreach (OdePrim prm in childrenPrim) + { + d.Mass m2; + d.MassSetZero(out m2); + d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z); - // rotate inertia - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; + d.Quaternion quat = new d.Quaternion(); + quat.W = prm._orientation.W; + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); + d.Matrix3 mat = new d.Matrix3(); + d.RfromQ(out mat, ref quat); + d.MassRotate(ref m2, ref mat); + d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z); + d.MassAdd(ref pMass, ref m2); + } - // set the body rotation - d.BodySetRotation(Body, ref mymat); + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - // recompute full object inertia if needed - if (childrenPrim.Count > 0) - { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - d.Mass tmpdmass = new d.Mass { }; - Vector3 rcm; +//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name); + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); - rcm.X = _position.X; - rcm.Y = _position.Y; - rcm.Z = _position.Z; + d.Quaternion quat = new d.Quaternion(); + quat.W = prm._orientation.W; + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) + d.Matrix3 mat = new d.Matrix3(); + d.RfromQ(out mat, ref quat); + if (Body != IntPtr.Zero) { - if (prm.prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); - continue; - } - - tmpdmass = prm.primdMass; - - // apply prim current rotation to inertia - quat.X = prm._orientation.X; - quat.Y = prm._orientation.Y; - quat.Z = prm._orientation.Z; - quat.W = prm._orientation.W; - d.RfromQ(out mat, ref quat); - d.MassRotate(ref tmpdmass, ref mat); - - Vector3 ppos = prm._position; - ppos.X -= rcm.X; - ppos.Y -= rcm.Y; - ppos.Z -= rcm.Z; - // refer inertia to root prim center of mass position - d.MassTranslate(ref tmpdmass, - ppos.X, - ppos.Y, - ppos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia - // fix prim colision cats - - if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); - } - - d.GeomClearOffset(prm.prim_geom); d.GeomSetBody(prm.prim_geom, Body); - prm.Body = Body; - d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + prm.childPrim = true; + d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); + //d.GeomSetOffsetPosition(prim.prim_geom, + // (Position.X - prm.Position.X) - pMass.c.X, + // (Position.Y - prm.Position.Y) - pMass.c.Y, + // (Position.Z - prm.Position.Z) - pMass.c.Z); + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); + //d.GeomSetOffsetRotation(prm.prim_geom, ref mat); + d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); + d.BodySetMass(Body, ref pMass); + } + else + { + m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name); } - } - } - - d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset - // associate root geom with body - d.GeomSetBody(prim_geom, Body); - - d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); - d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - myrot.X = -myrot.X; - myrot.Y = -myrot.Y; - myrot.Z = -myrot.Z; + prm.m_interpenetrationcount = 0; + prm.m_collisionscore = 0; + prm.m_disabled = false; - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) + { + prm.createAMotor(m_angularlock); + } + prm.Body = Body; + _parent_scene.ActivatePrim(prm); + } + + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + +//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); +//Console.WriteLine(" Post GeomSetCategoryBits 2"); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + d.Quaternion quat2 = new d.Quaternion(); + quat2.W = _orientation.W; + quat2.X = _orientation.X; + quat2.Y = _orientation.Y; + quat2.Z = _orientation.Z; + + d.Matrix3 mat2 = new d.Matrix3(); + d.RfromQ(out mat2, ref quat2); + d.GeomSetBody(prim_geom, Body); + d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z); + //d.GeomSetOffsetPosition(prim.prim_geom, + // (Position.X - prm.Position.X) - pMass.c.X, + // (Position.Y - prm.Position.Y) - pMass.c.Y, + // (Position.Z - prm.Position.Z) - pMass.c.Z); + //d.GeomSetOffsetRotation(prim_geom, ref mat2); + d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); + d.BodySetMass(Body, ref pMass); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + + m_interpenetrationcount = 0; + m_collisionscore = 0; + m_disabled = false; - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) + { + createAMotor(m_angularlock); + } - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode(Body, false); + d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - d.BodySetDamping(Body, .005f, .005f); + if (m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Enable(Body, _parent_scene); - if (m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - if (d.SpaceQuery(m_targetSpace, prim_geom)) - d.SpaceRemove(m_targetSpace, prim_geom); + _parent_scene.ActivatePrim(this); } + } + + private void ChildSetGeom(OdePrim odePrim) + { +// m_log.DebugFormat( +// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); - if (childrenPrim.Count == 0) + //if (IsPhysical && Body != IntPtr.Zero) + lock (childrenPrim) { - collide_geom = prim_geom; - m_targetSpace = _parent_scene.ActiveSpace; + foreach (OdePrim prm in childrenPrim) + { + //prm.childPrim = true; + prm.disableBody(); + //prm.m_taintparent = null; + //prm._parent = null; + //prm.m_taintPhysics = false; + //prm.m_disabled = true; + //prm.childPrim = false; + } } - else - { - m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); - d.HashSpaceSetLevels(m_targetSpace, -2, 8); - d.SpaceSetSublevel(m_targetSpace, 3); - d.SpaceSetCleanup(m_targetSpace, false); - d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(m_targetSpace, 0); - collide_geom = m_targetSpace; - } + disableBody(); - d.SpaceAdd(m_targetSpace, prim_geom); + // Spurious - Body == IntPtr.Zero after disableBody() +// if (Body != IntPtr.Zero) +// { +// _parent_scene.DeactivatePrim(this); +// } - if (m_delaySelect) + lock (childrenPrim) { - m_isSelected = true; - m_delaySelect = false; + foreach (OdePrim prm in childrenPrim) + { +//Console.WriteLine("ChildSetGeom calls ParentPrim"); + AddChildPrim(prm); + } } + } - m_collisionscore = 0; - - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - - _parent_scene.addActivePrim(this); + private void ChildDelink(OdePrim odePrim) + { +// m_log.DebugFormat( +// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); + // Okay, we have a delinked child.. need to rebuild the body. lock (childrenPrim) { foreach (OdePrim prm in childrenPrim) { - if (prm.prim_geom == IntPtr.Zero) - continue; - - Vector3 ppos = prm._position; - d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position + prm.childPrim = true; + prm.disableBody(); + //prm.m_taintparent = null; + //prm._parent = null; + //prm.m_taintPhysics = false; + //prm.m_disabled = true; + //prm.childPrim = false; + } + } - if (prm.m_targetSpace != m_targetSpace) - { - if (prm.m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); - if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) - d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); - } - prm.m_targetSpace = m_targetSpace; - d.SpaceAdd(m_targetSpace, prm.prim_geom); - } + disableBody(); - prm.m_collisionscore = 0; + lock (childrenPrim) + { + //Console.WriteLine("childrenPrim.Remove " + odePrim); + childrenPrim.Remove(odePrim); + } - if(!m_disabled) - prm.m_disabled = false; + // Spurious - Body == IntPtr.Zero after disableBody() +// if (Body != IntPtr.Zero) +// { +// _parent_scene.DeactivatePrim(this); +// } - _parent_scene.addActivePrim(prm); + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { +//Console.WriteLine("ChildDelink calls ParentPrim"); + AddChildPrim(prm); } } + } - // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + /// + /// Change prim in response to a selection taint. + /// + private void changeSelectedStatus() + { + if (m_taintselected) { - createAMotor(m_angularlock); - } + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + + // We do the body disable soft twice because 'in theory' a collision could have happened + // in between the disabling and the collision properties setting + // which would wake the physical body up from a soft disabling and potentially cause it to fall + // through the ground. + + // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select + // just one part of the assembly, the rest of the assembly is non-selected and still simulating, + // so that causes the selected part to wake up and continue moving. + + // even if you select all parts of a jointed assembly, it is not guaranteed that the entire + // assembly will stop simulating during the selection, because of the lack of atomicity + // of select operations (their processing could be interrupted by a thread switch, causing + // simulation to continue before all of the selected object notifications trickle down to + // the physics engine). + + // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are + // selected and disabled. then, due to a thread switch, the selection processing is + // interrupted and the physics engine continues to simulate, so the last 50 items, whose + // selection was not yet processed, continues to simulate. this wakes up ALL of the + // first 50 again. then the last 50 are disabled. then the first 50, which were just woken + // up, start simulating again, which in turn wakes up the last 50. + + if (IsPhysical) + { + disableBodySoft(); + } + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - if (m_isSelected || m_disabled) - { - d.BodyDisable(Body); + if (IsPhysical) + { + disableBodySoft(); + } } else { - d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - } - _parent_scene.addActiveGroups(this); - } + m_collisionCategories = CollisionCategories.Geom; - private void DestroyBody() - { - if (Body != IntPtr.Zero) - { - _parent_scene.remActivePrim(this); + if (IsPhysical) + m_collisionCategories |= CollisionCategories.Body; - collide_geom = IntPtr.Zero; + m_collisionFlags = m_default_collisionFlags; - if (m_disabled) - m_collisionCategories = 0; - else if (m_isSelected) - m_collisionCategories = CollisionCategories.Selected; - else if (m_isVolumeDetect) - m_collisionCategories = CollisionCategories.VolumeDtc; - else if (m_isphantom) - m_collisionCategories = CollisionCategories.Phantom; - else - m_collisionCategories = CollisionCategories.Geom; + if (m_collidesLand) + m_collisionFlags |= CollisionCategories.Land; + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; - m_collisionFlags = 0; + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - if (prim_geom != IntPtr.Zero) + if (IsPhysical) { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else + if (Body != IntPtr.Zero) { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); } - UpdateDataFromGeom(); - d.GeomSetBody(prim_geom, IntPtr.Zero); - SetInStaticSpace(this); } + } - if (!childPrim) + resetCollisionAccounting(); + m_isSelected = m_taintselected; + }//end changeSelectedStatus + + internal void ResetTaints() + { + m_taintposition = _position; + m_taintrot = _orientation; + m_taintPhysics = IsPhysical; + m_taintselected = m_isSelected; + m_taintsize = _size; + m_taintshape = false; + m_taintforce = false; + m_taintdisable = false; + m_taintVelocity = Vector3.Zero; + } + + /// + /// Create a geometry for the given mesh in the given target space. + /// + /// + /// If null, then a mesh is used that is based on the profile shape data. + private void CreateGeom(IntPtr m_targetSpace, IMesh mesh) + { +#if SPAM +Console.WriteLine("CreateGeom:"); +#endif + if (mesh != null) + { + setMesh(_parent_scene, mesh); + } + else + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) { - lock (childrenPrim) + if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) { - foreach (OdePrim prm in childrenPrim) + if (((_size.X / 2f) > 0f)) { - _parent_scene.remActivePrim(prm); - - if (prm.m_isSelected) - prm.m_collisionCategories = CollisionCategories.Selected; - else if (prm.m_isVolumeDetect) - prm.m_collisionCategories = CollisionCategories.VolumeDtc; - else if (prm.m_isphantom) - prm.m_collisionCategories = CollisionCategories.Phantom; - else - prm.m_collisionCategories = CollisionCategories.Geom; - - prm.m_collisionFlags = 0; - - if (prm.prim_geom != IntPtr.Zero) +// _parent_scene.waitForSpaceUnlock(m_targetSpace); + try { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); - } - prm.UpdateDataFromGeom(); - SetInStaticSpace(prm); +//Console.WriteLine(" CreateGeom 1"); + SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); + m_expectedCollisionContacts = 3; + } + catch (AccessViolationException) + { + m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); + return; + } + } + else + { +// _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { +//Console.WriteLine(" CreateGeom 2"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + m_expectedCollisionContacts = 4; + } + catch (AccessViolationException) + { + m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); + return; } - prm.Body = IntPtr.Zero; - prm._mass = prm.primMass; - prm.m_collisionscore = 0; } } - if (Amotor != IntPtr.Zero) + else { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; +// _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { +//Console.WriteLine(" CreateGeom 3"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + m_expectedCollisionContacts = 4; + } + catch (AccessViolationException) + { + m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); + return; + } + } + } + else + { +// _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { +//Console.WriteLine(" CreateGeom 4"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + m_expectedCollisionContacts = 4; + } + catch (AccessViolationException) + { + m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); + return; } - _parent_scene.remActiveGroup(this); - d.BodyDestroy(Body); } - Body = IntPtr.Zero; } - _mass = primMass; - m_collisionscore = 0; } - private void FixInertia(Vector3 NewPos,Quaternion newrot) + /// + /// Remove the existing geom from this prim. + /// + /// + /// If null, then a mesh is used that is based on the profile shape data. + /// true if the geom was successfully removed, false if it was already gone or the remove failed. + internal bool RemoveGeom() { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - - d.BodyGetMass(Body, out tmpdmass); - objdmass = tmpdmass; - - d.Vector3 dobjpos; - d.Vector3 thispos; - - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); - - // get prim own inertia in its local frame - tmpdmass = primdMass; - - // transform to object frame - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); + if (prim_geom != IntPtr.Zero) + { + try + { + _parent_scene.geom_name_map.Remove(prim_geom); + _parent_scene.actor_name_map.Remove(prim_geom); + d.GeomDestroy(prim_geom); + m_expectedCollisionContacts = 0; + prim_geom = IntPtr.Zero; + } + catch (System.AccessViolationException) + { + prim_geom = IntPtr.Zero; + m_expectedCollisionContacts = 0; + m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name); - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); + return false; + } - // back prim own inertia - tmpdmass = primdMass; + return true; + } + else + { + m_log.WarnFormat( + "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID); - // update to new position and orientation - _position = NewPos; - d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); - _orientation = newrot; - quat.X = newrot.X; - quat.Y = newrot.Y; - quat.Z = newrot.Z; - quat.W = newrot.W; - d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + return false; + } + } + /// + /// Add prim in response to an add taint. + /// + private void changeadd() + { +// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name); + + int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); + if (targetspace == IntPtr.Zero) + targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); + m_targetSpace = targetspace; - d.MassAdd(ref objdmass, ref tmpdmass); + IMesh mesh = null; - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) + if (_parent_scene.needsMeshing(_pbs)) { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); + // Don't need to re-enable body.. it's done in SetMesh + mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); + // createmesh returns null when it's a shape that isn't a cube. + // m_log.Debug(m_localID); + if (mesh == null) + CheckMeshAsset(); } - d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); - - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; - } +#if SPAM +Console.WriteLine("changeadd 1"); +#endif + CreateGeom(m_targetSpace, mesh); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); - private void FixInertia(Vector3 NewPos) - { - d.Matrix3 primmat = new d.Matrix3(); - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - d.Mass primmass = new d.Mass { }; + if (IsPhysical && Body == IntPtr.Zero) + enableBody(); - d.Vector3 dobjpos; - d.Vector3 thispos; + changeSelectedStatus(); - d.BodyGetMass(Body, out objdmass); + m_taintadd = false; + } - // get prim own inertia in its local frame - primmass = primdMass; - // transform to object frame - primmat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref primmass, ref primmat); + /// + /// Move prim in response to a move taint. + /// + private void changemove() + { + if (IsPhysical) + { + if (!m_disabled && !m_taintremove && !childPrim) + { + if (Body == IntPtr.Zero) + enableBody(); - tmpdmass = primmass; + //Prim auto disable after 20 frames, + //if you move it, re-enable the prim manually. + if (_parent != null) + { + if (m_linkJoint != IntPtr.Zero) + { + d.JointDestroy(m_linkJoint); + m_linkJoint = IntPtr.Zero; + } + } - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); + if (Body != IntPtr.Zero) + { + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); + if (_parent != null) + { + OdePrim odParent = (OdePrim)_parent; + if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) + { +// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? +Console.WriteLine(" JointCreateFixed"); + m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); + d.JointAttach(m_linkJoint, Body, odParent.Body); + d.JointSetFixed(m_linkJoint); + } + } + d.BodyEnable(Body); + if (m_vehicle.Type != Vehicle.TYPE_NONE) + { + m_vehicle.Enable(Body, _parent_scene); + } + } + else + { + m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name); + } + } + //else + // { + //m_log.Debug("[BUG]: race!"); + //} + } - // update to new position - _position = NewPos; - d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); +// _parent_scene.waitForSpaceUnlock(m_targetSpace); - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref primmass, - thispos.X, - thispos.Y, - thispos.Z); + IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); + m_targetSpace = tempspace; - d.MassAdd(ref objdmass, ref primmass); +// _parent_scene.waitForSpaceUnlock(m_targetSpace); - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) - { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); - } + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); +// _parent_scene.waitForSpaceUnlock(m_targetSpace); + d.SpaceAdd(m_targetSpace, prim_geom); - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); + changeSelectedStatus(); - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; + resetCollisionAccounting(); + m_taintposition = _position; } - private void FixInertia(Quaternion newrot) + internal void Move(float timestep) { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - d.Vector3 dobjpos; - d.Vector3 thispos; + float fx = 0; + float fy = 0; + float fz = 0; - d.BodyGetMass(Body, out objdmass); + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. + { + if (m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(timestep, _parent_scene); + } + else + { +//Console.WriteLine("Move " + Name); + if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 + // NON-'VEHICLES' are dealt with here +// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) +// { +// d.Vector3 avel2 = d.BodyGetAngularVel(Body); +// /* +// if (m_angularlock.X == 1) +// avel2.X = 0; +// if (m_angularlock.Y == 1) +// avel2.Y = 0; +// if (m_angularlock.Z == 1) +// avel2.Z = 0; +// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); +// */ +// } + //float PID_P = 900.0f; + + float m_mass = CalculateMass(); + +// fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); - // get prim own inertia in its local frame - tmpdmass = primdMass; - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - // transform to object frame - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); + + //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // gravityz multiplier = 1 - m_buoyancy + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); + if (m_usePID) + { +//Console.WriteLine("PID " + Name); + // KF - this is for object move? eg. llSetPos() ? + //if (!d.BodyIsEnabled(Body)) + //d.BodySetForce(Body, 0f, 0f, 0f); + // If we're using the PID controller, then we have no gravity + //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... + fz = 0f; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + //PidStatus = true; - // update to new orientation - _orientation = newrot; - quat.X = newrot.X; - quat.Y = newrot.Y; - quat.Z = newrot.Z; - quat.W = newrot.W; - d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + // PhysicsVector vec = new PhysicsVector(); + d.Vector3 vel = d.BodyGetLinearVel(Body); - tmpdmass = primdMass; - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); - d.MassAdd(ref objdmass, ref tmpdmass); + // if velocity is zero, use position control; otherwise, velocity control - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) - { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); - } + if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; - d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; - } + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end if (m_usePID) + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + if (m_useHoverPID && !m_usePID) + { +//Console.WriteLine("Hover " + Name); + + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; - #region Mass Calculation + // no lock; for now it's only called from within Simulate() - private void UpdatePrimBodyData() - { - primMass = m_density * primVolume; + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position - if (primMass <= 0) - primMass = 0.0001f;//ckrinke: Mass must be greater then zero. - if (primMass > _parent_scene.maximumMassObject) - primMass = _parent_scene.maximumMassObject; + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } - _mass = primMass; // just in case + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } - d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z); + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); - d.MassTranslate(ref primdMass, - m_OBBOffset.X, - m_OBBOffset.Y, - m_OBBOffset.Z); + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; - primOOBradiusSQ = m_OBB.LengthSquared(); + } // end switch (m_PIDHoverType) - if (_triMeshData != IntPtr.Zero) - { - float pc = m_physCost; - float psf = primOOBradiusSQ; - psf *= 1.33f * .2f; - pc *= psf; - if (pc < 0.1f) - pc = 0.1f; - m_physCost = pc; - } - else - m_physCost = 0.1f; + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); - m_streamCost = 1.0f; - } + // if velocity is zero, use position control; otherwise, velocity control - #endregion + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } - /// - /// Add a child prim to this parent prim. - /// - /// Child prim - // I'm the parent - // prim is the child - public void ParentPrim(OdePrim prim) - { - //Console.WriteLine("ParentPrim " + m_primName); - if (this.m_localID != prim.m_localID) - { - DestroyBody(); // for now we need to rebuil entire object on link change + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; - lock (childrenPrim) - { - // adopt the prim - if (!childrenPrim.Contains(prim)) - childrenPrim.Add(prim); + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; - // see if this prim has kids and adopt them also - // should not happen for now - foreach (OdePrim prm in prim.childrenPrim) + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) { - if (!childrenPrim.Contains(prm)) + //m_taintdisable = true; + //base.RaiseOutOfBounds(Position); + //d.BodySetLinearVel(Body, fx, fy, 0f); + if (!d.BodyIsEnabled(Body)) { - if (prm.Body != IntPtr.Zero) - { - if (prm.prim_geom != IntPtr.Zero) - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - if (prm.Body != prim.Body) - prm.DestroyBody(); // don't loose bodies around - prm.Body = IntPtr.Zero; - } - - childrenPrim.Add(prm); - prm._parent = this; + // A physical body at rest on a surface will auto-disable after a while, + // this appears to re-enable it incase the surface it is upon vanishes, + // and the body should fall again. + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); } + + // 35x10 = 350n times the mass per second applied maximum. + float nmax = 35f * m_mass; + float nmin = -35f * m_mass; + + if (fx > nmax) + fx = nmax; + if (fx < nmin) + fx = nmin; + if (fy > nmax) + fy = nmax; + if (fy < nmin) + fy = nmin; + d.BodyAddForce(Body, fx, fy, fz); +//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); } } - //Remove old children from the prim - prim.childrenPrim.Clear(); + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; +//Console.WriteLine("Nothing " + Name); + + } + } - if (prim.Body != IntPtr.Zero) + private void rotate() + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + if (Body != IntPtr.Zero) + { + // KF: If this is a root prim do BodySet + d.BodySetQuaternion(Body, ref myrot); + if (IsPhysical) { - if (prim.prim_geom != IntPtr.Zero) - d.GeomSetBody(prim.prim_geom, IntPtr.Zero); - prim.DestroyBody(); // don't loose bodies around - prim.Body = IntPtr.Zero; + if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); } - - prim.childPrim = true; - prim._parent = this; - - MakeBody(); // full nasty reconstruction } + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + resetCollisionAccounting(); + m_taintrot = _orientation; } - private void UpdateChildsfromgeom() + private void resetCollisionAccounting() { - if (childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.UpdateDataFromGeom(); - } + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_disabled = false; } - private void UpdateDataFromGeom() + /// + /// Change prim in response to a disable taint. + /// + private void changedisable() { - if (prim_geom != IntPtr.Zero) + m_disabled = true; + if (Body != IntPtr.Zero) { - d.Quaternion qtmp; - d.GeomCopyQuaternion(prim_geom, out qtmp); - _orientation.X = qtmp.X; - _orientation.Y = qtmp.Y; - _orientation.Z = qtmp.Z; - _orientation.W = qtmp.W; - -// Debug - float qlen = _orientation.Length(); - if (qlen > 1.01f || qlen < 0.99) - m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); -// - _orientation.Normalize(); - - d.Vector3 lpos = d.GeomGetPosition(prim_geom); - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; + d.BodyDisable(Body); + Body = IntPtr.Zero; } + + m_taintdisable = false; } - private void ChildDelink(OdePrim odePrim, bool remakebodies) + /// + /// Change prim in response to a physics status taint + /// + private void changePhysicsStatus() { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) // delinking the root prim + if (IsPhysical) { - OdePrim newroot = null; - lock (childrenPrim) + if (Body == IntPtr.Zero) { - if (childrenPrim.Count > 0) + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); + changeshape(); } - if (newroot != null) + else { - newroot.childPrim = false; - newroot._parent = null; - if (remakebodies) - newroot.MakeBody(); + enableBody(); } } } - else { - lock (childrenPrim) + if (Body != IntPtr.Zero) { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - // odePrim.UpdateDataFromGeom(); - if (remakebodies) - odePrim.MakeBody(); - } - } - if (remakebodies) - MakeBody(); - } - - protected void ChildRemove(OdePrim odePrim, bool reMakeBody) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + RemoveGeom(); - DestroyBody(); +//Console.WriteLine("changePhysicsStatus for " + Name); + changeadd(); + } - if (odePrim == this) - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) + if (childPrim) { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) + if (_parent != null) { - newroot.childrenPrim.Add(prm); + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); } - childrenPrim.Clear(); } - if (newroot != null) + else { - newroot.childPrim = false; - newroot._parent = null; - newroot.MakeBody(); + disableBody(); } } - if (reMakeBody) - MakeBody(); - return; - } - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - if (reMakeBody) - odePrim.MakeBody(); - } } - MakeBody(); - } - #region changes + changeSelectedStatus(); - private void changeadd() - { + resetCollisionAccounting(); + m_taintPhysics = IsPhysical; } - private void changeAngularLock(Vector3 newLock) + /// + /// Change prim in response to a size taint. + /// + private void changesize() { - // do we have a Physical object? - if (Body != IntPtr.Zero) +#if SPAM + m_log.DebugFormat("[ODE PRIM]: Called changesize"); +#endif + + if (_size.X <= 0) _size.X = 0.01f; + if (_size.Y <= 0) _size.Y = 0.01f; + if (_size.Z <= 0) _size.Z = 0.01f; + + //kill body to rebuild + if (IsPhysical && Body != IntPtr.Zero) { - //Check that we have a Parent - //If we have a parent then we're not authorative here - if (_parent == null) + if (childPrim) { - if (!newLock.ApproxEquals(Vector3.One, 0f)) - { - createAMotor(newLock); - } - else + if (_parent != null) { - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); } } - } - // Store this for later in case we get turned into a separate body - m_angularlock = newLock; - } - - private void changeLink(OdePrim NewParent) - { - if (_parent == null && NewParent != null) - { - NewParent.ParentPrim(this); - } - else if (_parent != null) - { - if (_parent is OdePrim) + else { - if (NewParent != _parent) - { - (_parent as OdePrim).ChildDelink(this, false); // for now... - childPrim = false; - - if (NewParent != null) - { - NewParent.ParentPrim(this); - } - } + disableBody(); } } - _parent = NewParent; - } - - - private void Stop() - { - if (!childPrim) - { - m_force = Vector3.Zero; - m_forceacc = Vector3.Zero; - m_angularForceacc = Vector3.Zero; - _torque = Vector3.Zero; - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - _target_velocity = Vector3.Zero; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - - _zeroFlag = false; - base.RequestPhysicsterseUpdate(); - } - if (Body != IntPtr.Zero) + if (d.SpaceQuery(m_targetSpace, prim_geom)) { - d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetTorque(Body, 0f, 0f, 0f); - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetAngularVel(Body, 0f, 0f, 0f); +// _parent_scene.waitForSpaceUnlock(m_targetSpace); + d.SpaceRemove(m_targetSpace, prim_geom); } - } - - private void changePhantomStatus(bool newval) - { - m_isphantom = newval; - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } + RemoveGeom(); -/* not in use - internal void ChildSelectedChange(bool childSelect) - { - if(childPrim) - return; + // we don't need to do space calculation because the client sends a position update also. - if (childSelect == m_isSelected) - return; + IMesh mesh = null; - if (childSelect) + // Construction of new prim + if (_parent_scene.needsMeshing(_pbs)) { - DoSelectedStatus(true); - } + float meshlod = _parent_scene.meshSculptLOD; - else - { - foreach (OdePrim prm in childrenPrim) + if (IsPhysical) + meshlod = _parent_scene.MeshSculptphysicalLOD; + // Don't need to re-enable body.. it's done in SetMesh + + if (_parent_scene.needsMeshing(_pbs)) { - if (prm.m_isSelected) - return; + mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); + if (mesh == null) + CheckMeshAsset(); } - DoSelectedStatus(false); + } - } -*/ - private void changeSelectedStatus(bool newval) - { - if (m_lastdoneSelected == newval) - return; - m_lastdoneSelected = newval; - DoSelectedStatus(newval); - } + CreateGeom(m_targetSpace, mesh); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); - private void CheckDelaySelect() - { - if (m_delaySelect) + //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); + if (IsPhysical && Body == IntPtr.Zero && !childPrim) { - DoSelectedStatus(m_isSelected); + // Re creates body on size. + // EnableBody also does setMass() + enableBody(); + d.BodyEnable(Body); } - } - private void DoSelectedStatus(bool newval) - { - m_isSelected = newval; - Stop(); + changeSelectedStatus(); - if (newval) + if (childPrim) { - if (!childPrim && Body != IntPtr.Zero) - d.BodyDisable(Body); - - if (m_delaySelect || m_isphysical) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = 0; - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != null) - { - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); - } - } - prm.m_delaySelect = false; - } - } -// else if (_parent != null) -// ((OdePrim)_parent).ChildSelectedChange(true); - - - if (prim_geom != null) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(collide_geom, 0); - d.GeomSetCollideBits(collide_geom, 0); - } - - } - else - { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); - if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); - } - } - } - - m_delaySelect = false; - } - else if(!m_isphysical) + if (_parent is OdePrim) { - m_delaySelect = true; + OdePrim parent = (OdePrim)_parent; + parent.ChildSetGeom(this); } } + resetCollisionAccounting(); + m_taintsize = _size; + } + + /// + /// Change prim in response to a float on water taint. + /// + /// + private void changefloatonwater() + { + m_collidesWater = m_taintCollidesWater; + + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } else { - if (!childPrim) - { - if (Body != IntPtr.Zero && !m_disabled) - d.BodyEnable(Body); - } -// else if (_parent != null) -// ((OdePrim)_parent).ChildSelectedChange(false); - - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - - m_delaySelect = false; + m_collisionFlags &= ~CollisionCategories.Water; } - resetCollisionAccounting(); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - private void changePosition(Vector3 newPos) + /// + /// Change prim in response to a shape taint. + /// + private void changeshape() { - CheckDelaySelect(); - if (m_isphysical) + m_taintshape = false; + + // Cleanup of old prim geometry and Bodies + if (IsPhysical && Body != IntPtr.Zero) { - if (childPrim) // inertia is messed, must rebuild + if (childPrim) { - if (m_building) - { - _position = newPos; - } - - else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) + if (_parent != null) { - FixInertia(newPos); - if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this); } } else { - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + disableBody(); } } - else + + RemoveGeom(); + + // we don't need to do space calculation because the client sends a position update also. + if (_size.X <= 0) _size.X = 0.01f; + if (_size.Y <= 0) _size.Y = 0.01f; + if (_size.Z <= 0) _size.Z = 0.01f; + // Construction of new prim + + IMesh mesh = null; + + + if (_parent_scene.needsMeshing(_pbs)) { - if (prim_geom != IntPtr.Zero) - { - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; + // Don't need to re-enable body.. it's done in CreateMesh + float meshlod = _parent_scene.meshSculptLOD; - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } + if (IsPhysical) + meshlod = _parent_scene.MeshSculptphysicalLOD; + + // createmesh returns null when it doesn't mesh. + mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); + if (mesh == null) + CheckMeshAsset(); } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; -// changeSelectedStatus(); - resetCollisionAccounting(); - } - private void changeOrientation(Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) + CreateGeom(m_targetSpace, mesh); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + //myrot.W = _orientation.w; + myrot.W = _orientation.W; + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + d.GeomSetQuaternion(prim_geom, ref myrot); + + //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); + if (IsPhysical && Body == IntPtr.Zero) { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _orientation = newOri; - } -/* - else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) - { - FixInertia(_position, newOri); - if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } -*/ - } - else + // Re creates body on size. + // EnableBody also does setMass() + enableBody(); + if (Body != IntPtr.Zero) { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + d.BodyEnable(Body); } } - else + + changeSelectedStatus(); + + if (childPrim) { - if (prim_geom != IntPtr.Zero) + if (_parent is OdePrim) { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } + OdePrim parent = (OdePrim)_parent; + parent.ChildSetGeom(this); } } - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; + resetCollisionAccounting(); +// m_taintshape = false; } - private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + /// + /// Change prim in response to an add force taint. + /// + private void changeAddForce() { - CheckDelaySelect(); - if (m_isphysical) + if (!m_isSelected) { - if (childPrim && m_building) // inertia is messed, must rebuild + lock (m_forcelist) { - _position = newPos; - _orientation = newOri; - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (_position != newPos) + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (IsPhysical) { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + Vector3 iforce = Vector3.Zero; + int i = 0; + try + { + for (i = 0; i < m_forcelist.Count; i++) + { + + iforce = iforce + (m_forcelist[i] * 100); + } + } + catch (IndexOutOfRangeException) + { + m_forcelist = new List(); + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_taintforce = false; + return; + } + catch (ArgumentOutOfRangeException) + { + m_forcelist = new List(); + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_taintforce = false; + return; + } d.BodyEnable(Body); + d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); + } + m_forcelist.Clear(); } - } - else - { - // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); - // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } + m_collisionscore = 0; + m_interpenetrationcount = 0; + } - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; + m_taintforce = false; + } - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } + /// + /// Change prim in response to a torque taint. + /// + private void changeSetTorque() + { + if (!m_isSelected) + { + if (IsPhysical && Body != IntPtr.Zero) + { + d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); } } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - resetCollisionAccounting(); + + m_taintTorque = Vector3.Zero; } - private void changeDisable(bool disable) + /// + /// Change prim in response to an angular force taint. + /// + private void changeAddAngularForce() { - if (disable) - { - if (!m_disabled) - disableBodySoft(); - } - else + if (!m_isSelected) { - if (m_disabled) - enableBodySoft(); + lock (m_angularforcelist) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (IsPhysical) + { + Vector3 iforce = Vector3.Zero; + for (int i = 0; i < m_angularforcelist.Count; i++) + { + iforce = iforce + (m_angularforcelist[i] * 100); + } + d.BodyEnable(Body); + d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); + + } + m_angularforcelist.Clear(); + } + + m_collisionscore = 0; + m_interpenetrationcount = 0; } + + m_taintaddangularforce = false; } - private void changePhysicsStatus(bool NewStatus) + /// + /// Change prim in response to a velocity taint. + /// + private void changevelocity() { - CheckDelaySelect(); - - m_isphysical = NewStatus; - - if (!childPrim) + if (!m_isSelected) { - if (NewStatus) - { - if (Body == IntPtr.Zero) - MakeBody(); - } - else + // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar + // walking through a default rez size prim if it keeps kicking it around - justincc. + Thread.Sleep(20); + + if (IsPhysical) { if (Body != IntPtr.Zero) { - DestroyBody(); + d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); } - Stop(); } + + //resetCollisionAccounting(); } - resetCollisionAccounting(); + m_taintVelocity = Vector3.Zero; } - private void changeSize(Vector3 newSize) + internal void setPrimForRemoval() { + m_taintremove = true; } - private void changeShape(PrimitiveBaseShape newShape) + public override bool Flying { + // no flying prims for you + get { return false; } + set { } } - private void changeAddPhysRep(ODEPhysRepData repData) + public override bool IsColliding { - _size = repData.size; //?? - _pbs = repData.pbs; - m_shapetype = repData.shapetype; - - m_mesh = repData.mesh; - - m_assetID = repData.assetID; - m_meshState = repData.meshState; + get { return iscolliding; } + set { iscolliding = value; } + } - m_hasOBB = repData.hasOBB; - m_OBBOffset = repData.OBBOffset; - m_OBB = repData.OBB; + public override bool CollidingGround + { + get { return false; } + set { return; } + } - primVolume = repData.volume; + public override bool CollidingObj + { + get { return false; } + set { return; } + } - CreateGeom(); + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - } + public override bool Stopped + { + get { return _zeroFlag; } + } - if (!m_isphysical) - { - SetInStaticSpace(this); - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - else - MakeBody(); + public override Vector3 Position + { + get { return _position; } - if ((m_meshState & MeshState.NeedMask) != 0) - { - repData.size = _size; - repData.pbs = _pbs; - repData.shapetype = m_shapetype; - _parent_scene.m_meshWorker.RequestMesh(repData); + set { _position = value; + //m_log.Info("[PHYSICS]: " + _position.ToString()); } } - private void changePhysRepData(ODEPhysRepData repData) + public override Vector3 Size { - CheckDelaySelect(); - - OdePrim parent = (OdePrim)_parent; - - bool chp = childPrim; - - if (chp) + get { return _size; } + set { - if (parent != null) + if (value.IsFinite()) { - parent.DestroyBody(); + _size = value; +// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); } } - else - { - DestroyBody(); - } - - RemoveGeom(); - - _size = repData.size; - _pbs = repData.pbs; - m_shapetype = repData.shapetype; - - m_mesh = repData.mesh; - - m_assetID = repData.assetID; - m_meshState = repData.meshState; - - m_hasOBB = repData.hasOBB; - m_OBBOffset = repData.OBBOffset; - m_OBB = repData.OBB; - - primVolume = repData.volume; - - CreateGeom(); + } - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - } + public override float Mass + { + get { return CalculateMass(); } + } - if (m_isphysical) + public override Vector3 Force + { + //get { return Vector3.Zero; } + get { return m_force; } + set { - if (chp) + if (value.IsFinite()) { - if (parent != null) - { - parent.MakeBody(); - } + m_force = value; } else - MakeBody(); - } - else - { - SetInStaticSpace(this); - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + } } + } - resetCollisionAccounting(); + public override int VehicleType + { + get { return (int)m_vehicle.Type; } + set { m_vehicle.ProcessTypeChange((Vehicle)value); } + } - if ((m_meshState & MeshState.NeedMask) != 0) - { - repData.size = _size; - repData.pbs = _pbs; - repData.shapetype = m_shapetype; - _parent_scene.m_meshWorker.RequestMesh(repData); - } + public override void VehicleFloatParam(int param, float value) + { + m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value); } - private void changeFloatOnWater(bool newval) + public override void VehicleVectorParam(int param, Vector3 value) { - m_collidesWater = newval; + m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value); + } - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); + public override void VehicleRotationParam(int param, Quaternion rotation) + { + m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); } - private void changeSetTorque(Vector3 newtorque) + public override void VehicleFlags(int param, bool remove) { - if (!m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + m_vehicle.ProcessVehicleFlags(param, remove); + } - } - _torque = newtorque; + public override void SetVolumeDetect(int param) + { + // We have to lock the scene here so that an entire simulate loop either uses volume detect for all + // possible collisions with this prim or for none of them. + lock (_parent_scene.OdeLock) + { + m_isVolumeDetect = (param != 0); } } - private void changeForce(Vector3 force) + public override Vector3 CenterOfMass { - m_force = force; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + get { return Vector3.Zero; } } - private void changeAddForce(Vector3 theforce) + public override Vector3 GeometricCenter { - m_forceacc += theforce; - if (!m_isSelected) - { - lock (this) - { - //m_log.Info("[PHYSICS]: dequeing forcelist"); - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - m_collisionscore = 0; - } + get { return Vector3.Zero; } } - // actually angular impulse - private void changeAddAngularImpulse(Vector3 aimpulse) + public override PrimitiveBaseShape Shape { - m_angularForceacc += aimpulse * m_invTimeStep; - if (!m_isSelected) + set { - lock (this) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - m_collisionscore = 0; + _pbs = value; + m_assetFailed = false; + m_taintshape = true; } } - private void changevelocity(Vector3 newVel) + public override Vector3 Velocity { - float len = newVel.LengthSquared(); - if (len > 100000.0f) // limit to 100m/s + get { - len = 100.0f / (float)Math.Sqrt(len); - newVel *= len; - } + // Average previous velocity with the new one so + // client object interpolation works a 'little' better + if (_zeroFlag) + return Vector3.Zero; - if (!m_isSelected) + Vector3 returnVelocity = Vector3.Zero; + returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2' + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f; + return returnVelocity; + } + set { - if (Body != IntPtr.Zero) + if (value.IsFinite()) { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + _velocity = value; - d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + m_taintVelocity = value; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); } - //resetCollisionAccounting(); + } - _velocity = newVel; } - private void changeangvelocity(Vector3 newAngVel) + public override Vector3 Torque { - float len = newAngVel.LengthSquared(); - if (len > 144.0f) // limit to 12rad/s + get { - len = 12.0f / (float)Math.Sqrt(len); - newAngVel *= len; + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return _torque; } - if (!m_isSelected) + set { - if (Body != IntPtr.Zero) + if (value.IsFinite()) { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - - d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); + m_taintTorque = value; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); } - //resetCollisionAccounting(); } - m_rotationalVelocity = newAngVel; } - private void changeVolumedetetion(bool newVolDtc) - { - m_isVolumeDetect = newVolDtc; - m_fakeisVolumeDetect = newVolDtc; - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - - protected void changeBuilding(bool newbuilding) + public override float CollisionScore { - // Check if we need to do anything - if (newbuilding == m_building) - return; - - if ((bool)newbuilding) - { - m_building = true; - if (!childPrim) - DestroyBody(); - } - else - { - m_building = false; - CheckDelaySelect(); - if (!childPrim) - MakeBody(); - } - if (!childPrim && childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.changeBuilding(m_building); // call directly - } + get { return m_collisionscore; } + set { m_collisionscore = value; } } - public void changeSetVehicle(VehicleData vdata) + public override bool Kinematic { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - m_vehicle.DoSetVehicle(vdata); + get { return false; } + set { } } - private void changeVehicleType(int value) + public override Quaternion Orientation { - if (value == (int)Vehicle.TYPE_NONE) - { - if (m_vehicle != null) - m_vehicle = null; - } - else + get { return _orientation; } + set { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - - m_vehicle.ProcessTypeChange((Vehicle)value); + if (QuaternionIsFinite(value)) + _orientation = value; + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); } } - private void changeVehicleFloatParam(strVehicleFloatParam fp) + private static bool QuaternionIsFinite(Quaternion q) { - if (m_vehicle == null) - return; - - m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; } - private void changeVehicleVectorParam(strVehicleVectorParam vp) + public override Vector3 Acceleration { - if (m_vehicle == null) - return; - m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + get { return _acceleration; } + set { _acceleration = value; } } - private void changeVehicleRotationParam(strVehicleQuatParam qp) + public override void AddForce(Vector3 force, bool pushforce) { - if (m_vehicle == null) - return; - m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); - } + if (force.IsFinite()) + { + lock (m_forcelist) + m_forcelist.Add(force); - private void changeVehicleFlags(strVehicleBoolParam bp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + m_taintforce = true; + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); } - private void changeBuoyancy(float b) + public override void AddAngularForce(Vector3 force, bool pushforce) { - m_buoyancy = b; + if (force.IsFinite()) + { + m_angularforcelist.Add(force); + m_taintaddangularforce = true; + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } } - private void changePIDTarget(Vector3 trg) + public override Vector3 RotationalVelocity { - m_PIDTarget = trg; - } + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; - private void changePIDTau(float tau) - { - m_PIDTau = tau; + if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } } - private void changePIDActive(bool val) + public override void CrossingFailure() { - m_usePID = val; + m_crossingfailures++; + if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + base.RaiseOutOfBounds(_position); + return; + } + else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name); + } } - private void changePIDHoverHeight(float val) + public override float Buoyancy { - m_PIDHoverHeight = val; - if (val == 0) - m_useHoverPID = false; + get { return m_buoyancy; } + set { m_buoyancy = value; } } - private void changePIDHoverType(PIDHoverType type) + public override void link(PhysicsActor obj) { - m_PIDHoverType = type; + m_taintparent = obj; } - private void changePIDHoverTau(float tau) + public override void delink() { - m_PIDHoverTau = tau; + m_taintparent = null; } - private void changePIDHoverActive(bool active) + public override void LockAngularMotion(Vector3 axis) { - m_useHoverPID = active; + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + m_taintAngularLock = axis; + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } } - #endregion - - public void Move() + internal void UpdatePositionAndVelocity() { - if (!childPrim && m_isphysical && Body != IntPtr.Zero && - !m_disabled && !m_isSelected && !m_building && !m_outbounds) + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null) { - if (!d.BodyIsEnabled(Body)) - { - // let vehicles sleep - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - return; - - if (++bodydisablecontrol < 20) - return; - - - d.BodyEnable(Body); - } - - bodydisablecontrol = 0; - - d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(); - return; - } - - float fx = 0; - float fy = 0; - float fz = 0; - - float m_mass = _mass; - - if (m_usePID && m_PIDTau > 0) - { - // for now position error - _target_velocity = - new Vector3( - (m_PIDTarget.X - lpos.X), - (m_PIDTarget.Y - lpos.Y), - (m_PIDTarget.Z - lpos.Z) - ); - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) - { - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - return; - } - else - { - _zeroFlag = false; - - float tmp = 1 / m_PIDTau; - _target_velocity *= tmp; - - // apply limits - tmp = _target_velocity.Length(); - if (tmp > 50.0f) - { - tmp = 50 / tmp; - _target_velocity *= tmp; - } - else if (tmp < 0.05f) - { - tmp = 0.05f / tmp; - _target_velocity *= tmp; - } - - d.Vector3 vel = d.BodyGetLinearVel(Body); - fx = (_target_velocity.X - vel.X) * m_invTimeStep; - fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; - fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; -// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); - } - } // end if (m_usePID) - - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) - { - - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); - - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - - case PIDHoverType.GroundAndWater: - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - else - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - break; - } // end switch (m_PIDHoverType) - - // don't go underground unless volumedetector - - if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + float m_minvelocity = 0; + if (Body != (IntPtr)0) // FIXME -> or if it is a joint + { + d.Vector3 vec = d.BodyGetPosition(Body); + d.Quaternion ori = d.BodyGetQuaternion(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + d.Vector3 torque = d.BodyGetTorque(Body); + _torque = new Vector3(torque.X, torque.Y, torque.Z); + Vector3 l_position = Vector3.Zero; + Quaternion l_orientation = Quaternion.Identity; + + // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) + //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } + + m_lastposition = _position; + m_lastorientation = _orientation; + + l_position.X = vec.X; + l_position.Y = vec.Y; + l_position.Z = vec.Z; + l_orientation.X = ori.X; + l_orientation.Y = ori.Y; + l_orientation.Z = ori.Z; + l_orientation.W = ori.W; + + if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) { - d.Vector3 vel = d.BodyGetLinearVel(Body); + //base.RaiseOutOfBounds(l_position); - fz = (m_targetHoverHeight - lpos.Z); - - // if error is zero, use position control; otherwise, velocity control - if (Math.Abs(fz) < 0.01f) + if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) { - d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + _position = l_position; + //_parent_scene.remActivePrim(this); + if (_parent == null) + base.RequestPhysicsterseUpdate(); + return; } else { - _zeroFlag = false; - fz /= m_PIDHoverTau; - - float tmp = Math.Abs(fz); - if (tmp > 50) - fz = 50 * Math.Sign(fz); - else if (tmp < 0.1) - fz = 0.1f * Math.Sign(fz); - - fz = ((fz - vel.Z) * m_invTimeStep); + if (_parent == null) + base.RaiseOutOfBounds(l_position); + return; } } - } - else - { - float b = (1.0f - m_buoyancy); - fx = _parent_scene.gravityx * b; - fy = _parent_scene.gravityy * b; - fz = _parent_scene.gravityz * b; - } - - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; - - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; - - m_forceacc = Vector3.Zero; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - d.BodyAddForce(Body, fx, fy, fz); - //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - - Vector3 trq; - - trq = _torque; - trq += m_angularForceacc; - m_angularForceacc = Vector3.Zero; - if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) - { - d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); - } - } - else - { // is not physical, or is not a body or is selected - // _zeroPosition = d.BodyGetPosition(Body); - return; - //Console.WriteLine("Nothing " + Name); - - } - } - - public void UpdatePositionAndVelocity() - { - if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) - { - if (d.BodyIsEnabled(Body) || !_zeroFlag) - { - bool lastZeroFlag = _zeroFlag; - - d.Vector3 lpos = d.GeomGetPosition(prim_geom); - // check outside region - if (lpos.Z < -100 || lpos.Z > 100000f) + if (l_position.Z < 0) { - m_outbounds = true; + // This is so prim that get lost underground don't fall forever and suck up + // + // Sim resources and memory. + // Disables the prim's movement physics.... + // It's a hack and will generate a console message if it fails. + + //IsPhysical = false; + if (_parent == null) + base.RaiseOutOfBounds(_position); - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); _acceleration.X = 0; _acceleration.Y = 0; _acceleration.Z = 0; @@ -3406,437 +2698,589 @@ namespace OpenSim.Region.Physics.OdePlugin m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - -// throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - return; - } - - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } - - if (m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; - - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - UnSubscribeEvents(); - - base.RequestPhysicsterseUpdate(); - return; + if (_parent == null) + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + //outofBounds = true; } - d.Quaternion ori; - d.GeomCopyQuaternion(prim_geom, out ori); - - // decide if moving - // use positions since this are integrated quantities - // tolerance values depende a lot on simulation noise... - // use simple math.abs since we dont need to be exact - - if ( - (Math.Abs(_position.X - lpos.X) < 0.001f) - && (Math.Abs(_position.Y - lpos.Y) < 0.001f) - && (Math.Abs(_position.Z - lpos.Z) < 0.001f) - && (Math.Abs(_orientation.X - ori.X) < 0.0001f) - && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f) - && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W - ) + //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); +//Console.WriteLine("Adiff " + Name + " = " + Adiff); + if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) +// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) + && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large { _zeroFlag = true; +//Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; } else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } - // update velocities and aceleration - if (!(_zeroFlag && lastZeroFlag)) + if (_zeroFlag) { - d.Vector3 vel = d.BodyGetLinearVel(Body); + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; - _acceleration = _velocity; + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; - if ((Math.Abs(vel.X) < 0.001f) && - (Math.Abs(vel.Y) < 0.001f) && - (Math.Abs(vel.Z) < 0.001f)) + //_orientation.w = 0f; + //_orientation.X = 0f; + //_orientation.Y = 0f; + //_orientation.Z = 0f; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) { - _velocity = Vector3.Zero; - float t = -m_invTimeStep; - _acceleration = _acceleration * t; + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastUpdateSent = true; } - else + } + else + { + if (lastZeroFlag != _zeroFlag) { - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - _acceleration = (_velocity - _acceleration) * m_invTimeStep; + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } } - if ((Math.Abs(_acceleration.X) < 0.01f) && - (Math.Abs(_acceleration.Y) < 0.01f) && - (Math.Abs(_acceleration.Z) < 0.01f)) + m_lastVelocity = _velocity; + + _position = l_position; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _acceleration = ((_velocity - m_lastVelocity) / 0.1f); + _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); + //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); + + // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... + // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. + // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles + // adding these logical exclusion situations to maintain this where I think it was intended to be. + if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) + { + m_minvelocity = 0.5f; + } + else { - _acceleration = Vector3.Zero; + m_minvelocity = 0.02f; } - if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && - (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && - (Math.Abs(_orientation.Z - ori.Z) < 0.0001) - ) + if (_velocity.ApproxEquals(pv, m_minvelocity)) { - m_rotationalVelocity = Vector3.Zero; + m_rotationalVelocity = pv; } else { - vel = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = vel.X; - m_rotationalVelocity.Y = vel.Y; - m_rotationalVelocity.Z = vel.Z; + m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); } - } - if (_zeroFlag) - { - if (lastZeroFlag) + //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) { - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; + if (_parent == null) + { + base.RequestPhysicsterseUpdate(); + } } - - if (!m_lastUpdateSent) + else { - base.RequestPhysicsterseUpdate(); - if (lastZeroFlag) - m_lastUpdateSent = true; + throttleCounter++; } - return; } + m_lastposition = l_position; + } + else + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - base.RequestPhysicsterseUpdate(); - m_lastUpdateSent = false; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; } } } - internal static bool QuaternionIsFinite(Quaternion q) + public override bool FloatOnWater { - if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) - return false; - if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) - return false; - if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) - return false; - if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) - return false; - return true; + set { + m_taintCollidesWater = value; + _parent_scene.AddPhysicsActorTaint(this); + } } - internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) + public override void SetMomentum(Vector3 momentum) { - // assumes object center of mass is zero - float smass = part.mass; - theobj.mass -= smass; + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + public override Quaternion APIDTarget{ set { return; } } - smass *= 1.0f / (theobj.mass); ; + public override bool APIDActive{ set { return; } } - theobj.c.X -= part.c.X * smass; - theobj.c.Y -= part.c.Y * smass; - theobj.c.Z -= part.c.Z * smass; + public override float APIDStrength{ set { return; } } - theobj.I.M00 -= part.I.M00; - theobj.I.M01 -= part.I.M01; - theobj.I.M02 -= part.I.M02; - theobj.I.M10 -= part.I.M10; - theobj.I.M11 -= part.I.M11; - theobj.I.M12 -= part.I.M12; - theobj.I.M20 -= part.I.M20; - theobj.I.M21 -= part.I.M21; - theobj.I.M22 -= part.I.M22; - } + public override float APIDDamping{ set { return; } } - private void donullchange() + private void createAMotor(Vector3 axis) { - } + if (Body == IntPtr.Zero) + return; - public bool DoAChange(changes what, object arg) - { - if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) + if (Amotor != IntPtr.Zero) { - return false; + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; } - // nasty switch - switch (what) - { - case changes.Add: - changeadd(); - break; - - case changes.AddPhysRep: - changeAddPhysRep((ODEPhysRepData)arg); - break; - - case changes.Remove: - //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... - //When we return true, it destroys all of the prims in the linkset anyway - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildRemove(this, false); - } - else - ChildRemove(this, false); + float axisnum = 3; - m_vehicle = null; - RemoveGeom(); - m_targetSpace = IntPtr.Zero; - UnSubscribeEvents(); - return true; + axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); - case changes.Link: - OdePrim tmp = (OdePrim)arg; - changeLink(tmp); - break; + // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); - case changes.DeLink: - changeLink(null); - break; + + // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. + d.Mass objMass; + d.MassSetZero(out objMass); + DMassCopy(ref pMass, ref objMass); - case changes.Position: - changePosition((Vector3)arg); - break; + //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); - case changes.Orientation: - changeOrientation((Quaternion)arg); - break; + Matrix4 dMassMat = FromDMass(objMass); - case changes.PosOffset: - donullchange(); - break; + Matrix4 mathmat = Inverse(dMassMat); - case changes.OriOffset: - donullchange(); - break; + /* + //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]); - case changes.Velocity: - changevelocity((Vector3)arg); - break; + mathmat = Inverse(mathmat); -// case changes.Acceleration: -// changeacceleration((Vector3)arg); -// break; - case changes.AngVelocity: - changeangvelocity((Vector3)arg); - break; + objMass = FromMatrix4(mathmat, ref objMass); + //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); - case changes.Force: - changeForce((Vector3)arg); - break; + mathmat = Inverse(mathmat); + */ + if (axis.X == 0) + { + mathmat.M33 = 50.0000001f; + //objMass.I.M22 = 0; + } + if (axis.Y == 0) + { + mathmat.M22 = 50.0000001f; + //objMass.I.M11 = 0; + } + if (axis.Z == 0) + { + mathmat.M11 = 50.0000001f; + //objMass.I.M00 = 0; + } + + - case changes.Torque: - changeSetTorque((Vector3)arg); - break; + mathmat = Inverse(mathmat); + objMass = FromMatrix4(mathmat, ref objMass); + //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); + + //return; + if (d.MassCheck(ref objMass)) + { + d.BodySetMass(Body, ref objMass); + } + else + { + //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); + } - case changes.AddForce: - changeAddForce((Vector3)arg); - break; + if (axisnum <= 0) + return; + // int dAMotorEuler = 1; - case changes.AddAngForce: - changeAddAngularImpulse((Vector3)arg); - break; + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + d.JointSetAMotorMode(Amotor, 0); - case changes.AngLock: - changeAngularLock((Vector3)arg); - break; + d.JointSetAMotorNumAxes(Amotor,(int)axisnum); + int i = 0; - case changes.Size: - changeSize((Vector3)arg); - break; + if (axis.X == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); + i++; + } - case changes.Shape: - changeShape((PrimitiveBaseShape)arg); - break; + if (axis.Y == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); + i++; + } - case changes.PhysRepData: - changePhysRepData((ODEPhysRepData) arg); - break; + if (axis.Z == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); + i++; + } - case changes.CollidesWater: - changeFloatOnWater((bool)arg); - break; + for (int j = 0; j < (int)axisnum; j++) + { + //d.JointSetAMotorAngle(Amotor, j, 0); + } - case changes.VolumeDtc: - changeVolumedetetion((bool)arg); - break; + //d.JointSetAMotorAngle(Amotor, 1, 0); + //d.JointSetAMotorAngle(Amotor, 2, 0); - case changes.Phantom: - changePhantomStatus((bool)arg); - break; + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); + //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f); + d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// + } - case changes.Physical: - changePhysicsStatus((bool)arg); - break; + private Matrix4 FromDMass(d.Mass pMass) + { + Matrix4 obj; + obj.M11 = pMass.I.M00; + obj.M12 = pMass.I.M01; + obj.M13 = pMass.I.M02; + obj.M14 = 0; + obj.M21 = pMass.I.M10; + obj.M22 = pMass.I.M11; + obj.M23 = pMass.I.M12; + obj.M24 = 0; + obj.M31 = pMass.I.M20; + obj.M32 = pMass.I.M21; + obj.M33 = pMass.I.M22; + obj.M34 = 0; + obj.M41 = 0; + obj.M42 = 0; + obj.M43 = 0; + obj.M44 = 1; + return obj; + } - case changes.Selected: - changeSelectedStatus((bool)arg); - break; + private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) + { + obj.I.M00 = pMat[0, 0]; + obj.I.M01 = pMat[0, 1]; + obj.I.M02 = pMat[0, 2]; + obj.I.M10 = pMat[1, 0]; + obj.I.M11 = pMat[1, 1]; + obj.I.M12 = pMat[1, 2]; + obj.I.M20 = pMat[2, 0]; + obj.I.M21 = pMat[2, 1]; + obj.I.M22 = pMat[2, 2]; + return obj; + } - case changes.disabled: - changeDisable((bool)arg); - break; + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + } - case changes.building: - changeBuilding((bool)arg); - break; + public override void UnSubscribeEvents() + { + _parent_scene.RemoveCollisionEventReporting(this); + m_eventsubscription = 0; + } - case changes.VehicleType: - changeVehicleType((int)arg); - break; + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } - case changes.VehicleFlags: - changeVehicleFlags((strVehicleBoolParam) arg); - break; + public void SendCollisions() + { + if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); - case changes.VehicleFloatParam: - changeVehicleFloatParam((strVehicleFloatParam) arg); - break; + if (CollisionEventsThisFrame.Count > 0) + { + m_collisionsOnPreviousFrame = true; + CollisionEventsThisFrame.Clear(); + } + else + { + m_collisionsOnPreviousFrame = false; + } + } + } - case changes.VehicleVectorParam: - changeVehicleVectorParam((strVehicleVectorParam) arg); - break; + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } - case changes.VehicleRotationParam: - changeVehicleRotationParam((strVehicleQuatParam) arg); - break; + public static Matrix4 Inverse(Matrix4 pMat) + { + if (determinant3x3(pMat) == 0) + { + return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible + } - case changes.SetVehicle: - changeSetVehicle((VehicleData) arg); - break; + return (Adjoint(pMat) / determinant3x3(pMat)); + } - case changes.Buoyancy: - changeBuoyancy((float)arg); - break; + public static Matrix4 Adjoint(Matrix4 pMat) + { + Matrix4 adjointMatrix = new Matrix4(); + for (int i=0; i<4; i++) + { + for (int j=0; j<4; j++) + { + Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); + } + } - case changes.PIDTarget: - changePIDTarget((Vector3)arg); - break; + adjointMatrix = Transpose(adjointMatrix); + return adjointMatrix; + } - case changes.PIDTau: - changePIDTau((float)arg); - break; + public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) + { + Matrix4 minor = new Matrix4(); + int m = 0, n = 0; + for (int i = 0; i < 4; i++) + { + if (i == iRow) + continue; + n = 0; + for (int j = 0; j < 4; j++) + { + if (j == iCol) + continue; + Matrix4SetValue(ref minor, m,n, matrix[i, j]); + n++; + } + m++; + } - case changes.PIDActive: - changePIDActive((bool)arg); - break; + return minor; + } - case changes.PIDHoverHeight: - changePIDHoverHeight((float)arg); - break; + public static Matrix4 Transpose(Matrix4 pMat) + { + Matrix4 transposeMatrix = new Matrix4(); + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); + return transposeMatrix; + } - case changes.PIDHoverType: - changePIDHoverType((PIDHoverType)arg); - break; + public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) + { + switch (r) + { + case 0: + switch (c) + { + case 0: + pMat.M11 = val; + break; + case 1: + pMat.M12 = val; + break; + case 2: + pMat.M13 = val; + break; + case 3: + pMat.M14 = val; + break; + } - case changes.PIDHoverTau: - changePIDHoverTau((float)arg); break; + case 1: + switch (c) + { + case 0: + pMat.M21 = val; + break; + case 1: + pMat.M22 = val; + break; + case 2: + pMat.M23 = val; + break; + case 3: + pMat.M24 = val; + break; + } - case changes.PIDHoverActive: - changePIDHoverActive((bool)arg); break; + case 2: + switch (c) + { + case 0: + pMat.M31 = val; + break; + case 1: + pMat.M32 = val; + break; + case 2: + pMat.M33 = val; + break; + case 3: + pMat.M34 = val; + break; + } - case changes.Null: - donullchange(); break; + case 3: + switch (c) + { + case 0: + pMat.M41 = val; + break; + case 1: + pMat.M42 = val; + break; + case 2: + pMat.M43 = val; + break; + case 3: + pMat.M44 = val; + break; + } - - - default: - donullchange(); break; } - return false; } - public void AddChange(changes what, object arg) + private static float determinant3x3(Matrix4 pMat) { - _parent_scene.AddChange((PhysicsActor) this, what, arg); - } - + float det = 0; + float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; + float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; + float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; + float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; + float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; + float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; - private struct strVehicleBoolParam - { - public int param; - public bool value; + det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); + return det; + } + + private static void DMassCopy(ref d.Mass src, ref d.Mass dst) + { + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; } - private struct strVehicleFloatParam + public override void SetMaterial(int pMaterial) { - public int param; - public float value; + m_material = pMaterial; } - private struct strVehicleQuatParam + private void CheckMeshAsset() { - public int param; - public Quaternion value; + if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero) + { + m_assetFailed = true; + Util.FireAndForget(delegate + { + RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod; + if (assetProvider != null) + assetProvider(_pbs.SculptTexture, MeshAssetReveived); + }); + } } - private struct strVehicleVectorParam + void MeshAssetReveived(AssetBase asset) { - public int param; - public Vector3 value; - } + if (asset.Data != null && asset.Data.Length > 0) + { + if (!_pbs.SculptEntry) + return; + if (_pbs.SculptTexture.ToString() != asset.ID) + return; + + _pbs.SculptData = new byte[asset.Data.Length]; + asset.Data.CopyTo(_pbs.SculptData, 0); + m_assetFailed = false; + m_taintshape = true; + _parent_scene.AddPhysicsActorTaint(this); + } + } } -} +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 03048a4..7a50c4c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -25,21 +25,26 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +//#define USE_DRAWSTUFF //#define SPAM using System; using System.Collections.Generic; +using System.Diagnostics; +using System.IO; +using System.Linq; using System.Reflection; using System.Runtime.InteropServices; using System.Threading; -using System.IO; -using System.Diagnostics; using log4net; using Nini.Config; -using OdeAPI; +using Ode.NET; +using OpenMetaverse; +#if USE_DRAWSTUFF +using Drawstuff.NET; +#endif using OpenSim.Framework; using OpenSim.Region.Physics.Manager; -using OpenMetaverse; namespace OpenSim.Region.Physics.OdePlugin { @@ -50,42 +55,29 @@ namespace OpenSim.Region.Physics.OdePlugin End = 2 } - public struct sCollisionData - { - public uint ColliderLocalId; - public uint CollidedWithLocalId; - public int NumberOfCollisions; - public int CollisionType; - public int StatusIndicator; - public int lastframe; - } - - - // colision flags of things others can colide with - // rays, sensors, probes removed since can't be colided with - // The top space where things are placed provided further selection - // ie physical are in active space nonphysical in static - // this should be exclusive as possible +// public struct sCollisionData +// { +// public uint ColliderLocalId; +// public uint CollidedWithLocalId; +// public int NumberOfCollisions; +// public int CollisionType; +// public int StatusIndicator; +// public int lastframe; +// } [Flags] - public enum CollisionCategories : uint + public enum CollisionCategories : int { Disabled = 0, - //by 'things' types - Space = 0x01, - Geom = 0x02, // aka prim/part - Character = 0x04, - Land = 0x08, - Water = 0x010, - - // by state - Phantom = 0x01000, - VolumeDtc = 0x02000, - Selected = 0x04000, - NoShape = 0x08000, - - - All = 0xffffffff + Geom = 0x00000001, + Body = 0x00000002, + Space = 0x00000004, + Character = 0x00000008, + Land = 0x00000010, + Water = 0x00000020, + Wind = 0x00000040, + Sensor = 0x00000080, + Selected = 0x00000100 } /// @@ -106,213 +98,400 @@ namespace OpenSim.Region.Physics.OdePlugin /// Plastic = 5, /// - Rubber = 6, - - light = 7 // compatibility with old viewers - } - - public enum changes : int - { - Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) - Remove, - Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root - // or removes from a object if arg is null - DeLink, - Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child - Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child - PosOffset, // not in use - // arg Vector3 new position in local coords. Changes prim position in object - OriOffset, // not in use - // arg Vector3 new position in local coords. Changes prim position in object - Velocity, - AngVelocity, - Acceleration, - Force, - Torque, - Momentum, - - AddForce, - AddAngForce, - AngLock, - - Buoyancy, - - PIDTarget, - PIDTau, - PIDActive, - - PIDHoverHeight, - PIDHoverType, - PIDHoverTau, - PIDHoverActive, - - Size, - Shape, - PhysRepData, - AddPhysRep, - - CollidesWater, - VolumeDtc, - - Physical, - Phantom, - Selected, - disabled, - building, - - VehicleType, - VehicleFloatParam, - VehicleVectorParam, - VehicleRotationParam, - VehicleFlags, - SetVehicle, - - Null //keep this last used do dim the methods array. does nothing but pulsing the prim + Rubber = 6 } - public struct ODEchangeitem - { - public PhysicsActor actor; - public OdeCharacter character; - public changes what; - public Object arg; - } - public class OdeScene : PhysicsScene { private readonly ILog m_log; // private Dictionary m_storedCollisions = new Dictionary(); - public bool OdeUbitLib = false; -// private int threadid = 0; - private Random fluidRandomizer = new Random(Environment.TickCount); + /// + /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances. + /// + /// + /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a + /// time, even where physics objects are in entirely different ODE worlds. This is because generating contacts + /// uses a static cache at the ODE level. + /// + /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar + /// to + /// + /// mono() [0x489171] + /// mono() [0x4d154f] + /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60] + /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a] + /// + /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option + /// causes OpenSimulator to immediately crash with a native stack trace similar to + /// + /// mono() [0x489171] + /// mono() [0x4d154f] + /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60] + /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82] + /// + internal static Object UniversalColliderSyncObject = new Object(); + + /// + /// Is stats collecting enabled for this ODE scene? + /// + public bool CollectStats { get; set; } + + /// + /// Statistics for this scene. + /// + private Dictionary m_stats = new Dictionary(); + + /// + /// Stat name for total number of avatars in this ODE scene. + /// + public const string ODETotalAvatarsStatName = "ODETotalAvatars"; + + /// + /// Stat name for total number of prims in this ODE scene. + /// + public const string ODETotalPrimsStatName = "ODETotalPrims"; + + /// + /// Stat name for total number of prims with active physics in this ODE scene. + /// + public const string ODEActivePrimsStatName = "ODEActivePrims"; + + /// + /// Stat name for the total time spent in ODE frame processing. + /// + /// + /// A sanity check for the main scene loop physics time. + /// + public const string ODETotalFrameMsStatName = "ODETotalFrameMS"; + + /// + /// Stat name for time spent processing avatar taints per frame + /// + public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS"; + + /// + /// Stat name for time spent processing prim taints per frame + /// + public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS"; + + /// + /// Stat name for time spent calculating avatar forces per frame. + /// + public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS"; + + /// + /// Stat name for time spent calculating prim forces per frame + /// + public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS"; + + /// + /// Stat name for time spent fulfilling raycasting requests per frame + /// + public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS"; + + /// + /// Stat name for time spent in native code that actually steps through the simulation. + /// + public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS"; + + /// + /// Stat name for the number of milliseconds that ODE spends in native space collision code. + /// + public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS"; - const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; - const float MaxERP = 0.8f; - const float minERP = 0.1f; - const float comumContactCFM = 0.0001f; - - float frictionMovementMult = 0.8f; + /// + /// Stat name for milliseconds that ODE spends in native geom collision code. + /// + public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS"; + + /// + /// Time spent in collision processing that is not spent in native space or geom collision code. + /// + public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS"; + + /// + /// Stat name for time spent notifying listeners of collisions + /// + public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS"; + + /// + /// Stat name for milliseconds spent updating avatar position and velocity + /// + public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS"; + + /// + /// Stat name for the milliseconds spent updating prim position and velocity + /// + public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS"; + + /// + /// Stat name for avatar collisions with another entity. + /// + public const string ODEAvatarContactsStatsName = "ODEAvatarContacts"; + + /// + /// Stat name for prim collisions with another entity. + /// + public const string ODEPrimContactsStatName = "ODEPrimContacts"; + + /// + /// Used to hold tick numbers for stat collection purposes. + /// + private int m_nativeCollisionStartTick; + + /// + /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback. + /// + private bool m_inCollisionTiming; + + /// + /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object + /// collisions occured using the _perloopcontact if stats collection is enabled. + /// + private int m_tempAvatarCollisionsThisFrame; + + /// + /// Used in calculating physics frame time dilation + /// + private int tickCountFrameRun; - float TerrainBounce = 0.1f; - float TerrainFriction = 0.3f; + /// + /// Used in calculating physics frame time dilation + /// + private int latertickcount; - public float AvatarFriction = 0;// 0.9f * 0.5f; + private Random fluidRandomizer = new Random(Environment.TickCount); private const uint m_regionWidth = Constants.RegionSize; private const uint m_regionHeight = Constants.RegionSize; - public float ODE_STEPSIZE = 0.020f; - public float HalfOdeStep = 0.01f; - public int odetimestepMS = 20; // rounded - private float metersInSpace = 25.6f; + private float ODE_STEPSIZE = 0.0178f; + private float metersInSpace = 29.9f; private float m_timeDilation = 1.0f; - private DateTime m_lastframe; - private DateTime m_lastMeshExpire; - public float gravityx = 0f; public float gravityy = 0f; public float gravityz = -9.8f; + public float AvatarTerminalVelocity { get; set; } + + private float contactsurfacelayer = 0.001f; + + private int worldHashspaceLow = -4; + private int worldHashspaceHigh = 128; + + private int smallHashspaceLow = -4; + private int smallHashspaceHigh = 66; + private float waterlevel = 0f; private int framecount = 0; + //private int m_returncollisions = 10; - private int m_meshExpireCntr; + private readonly IntPtr contactgroup; -// private IntPtr WaterGeom = IntPtr.Zero; -// private IntPtr WaterHeightmapData = IntPtr.Zero; -// private GCHandle WaterMapHandler = new GCHandle(); + internal IntPtr WaterGeom; - public float avPIDD = 2200f; // make it visible - public float avPIDP = 900f; // make it visible + private float nmTerrainContactFriction = 255.0f; + private float nmTerrainContactBounce = 0.1f; + private float nmTerrainContactERP = 0.1025f; + + private float mTerrainContactFriction = 75f; + private float mTerrainContactBounce = 0.1f; + private float mTerrainContactERP = 0.05025f; + + private float nmAvatarObjectContactFriction = 250f; + private float nmAvatarObjectContactBounce = 0.1f; + + private float mAvatarObjectContactFriction = 75f; + private float mAvatarObjectContactBounce = 0.1f; + + private float avPIDD = 3200f; + private float avPIDP = 1400f; private float avCapRadius = 0.37f; - private float avDensity = 3f; + private float avStandupTensor = 2000000f; + + /// + /// true = old compatibility mode with leaning capsule; false = new corrected mode + /// + /// + /// Even when set to false, the capsule still tilts but this is done in a different way. + /// + public bool IsAvCapsuleTilted { get; private set; } + + private float avDensity = 80f; +// private float avHeightFudgeFactor = 0.52f; private float avMovementDivisorWalk = 1.3f; private float avMovementDivisorRun = 0.8f; private float minimumGroundFlightOffset = 3f; public float maximumMassObject = 10000.01f; + public bool meshSculptedPrim = true; + public bool forceSimplePrimMeshing = false; + + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 16; public float geomDefaultDensity = 10.000006836f; public int geomContactPointsStartthrottle = 3; public int geomUpdatesPerThrottledUpdate = 15; + private const int avatarExpectedContacts = 3; public float bodyPIDD = 35f; public float bodyPIDG = 25; -// public int geomCrossingFailuresBeforeOutofbounds = 6; + public int geomCrossingFailuresBeforeOutofbounds = 5; + + public float bodyMotorJointMaxforceTensor = 2; - public int bodyFramesAutoDisable = 5; + public int bodyFramesAutoDisable = 20; + private float[] _watermap; + private bool m_filterCollisions = true; private d.NearCallback nearCallback; + public d.TriCallback triCallback; + public d.TriArrayCallback triArrayCallback; - private HashSet _characters = new HashSet(); - private HashSet _prims = new HashSet(); - private HashSet _activeprims = new HashSet(); - private HashSet _activegroups = new HashSet(); + /// + /// Avatars in the physics scene. + /// + private readonly HashSet _characters = new HashSet(); - public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); + /// + /// Prims in the physics scene. + /// + private readonly HashSet _prims = new HashSet(); /// - /// A list of actors that should receive collision events. + /// Prims in the physics scene that are subject to physics, not just collisions. /// - private List _collisionEventPrim = new List(); - private List _collisionEventPrimRemove = new List(); - - private HashSet _badCharacter = new HashSet(); -// public Dictionary geom_name_map = new Dictionary(); - public Dictionary actor_name_map = new Dictionary(); + private readonly HashSet _activeprims = new HashSet(); + + /// + /// Prims that the simulator has created/deleted/updated and so need updating in ODE. + /// + private readonly HashSet _taintedPrims = new HashSet(); - private float contactsurfacelayer = 0.002f; + /// + /// Record a character that has taints to be processed. + /// + private readonly HashSet _taintedActors = new HashSet(); - private int contactsPerCollision = 80; - internal IntPtr ContactgeomsArray = IntPtr.Zero; - private IntPtr GlobalContactsArray = IntPtr.Zero; + /// + /// Keep record of contacts in the physics loop so that we can remove duplicates. + /// + private readonly List _perloopContact = new List(); - const int maxContactsbeforedeath = 4000; - private volatile int m_global_contactcount = 0; + /// + /// A dictionary of actors that should receive collision events. + /// + private readonly Dictionary m_collisionEventActors = new Dictionary(); - private IntPtr contactgroup; + /// + /// A dictionary of collision event changes that are waiting to be processed. + /// + private readonly Dictionary m_collisionEventActorsChanges = new Dictionary(); - public ContactData[] m_materialContactsData = new ContactData[8]; + /// + /// Maps a unique geometry id (a memory location) to a physics actor name. + /// + /// + /// Only actors participating in collisions have geometries. This has to be maintained separately from + /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own + /// apart from the singleton PANull + /// + public Dictionary geom_name_map = new Dictionary(); - private Dictionary RegionTerrain = new Dictionary(); - private Dictionary TerrainHeightFieldHeights = new Dictionary(); - private Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); - - private int m_physicsiterations = 10; - private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag -// private PhysicsActor PANull = new NullPhysicsActor(); - private float step_time = 0.0f; + /// + /// Maps a unique geometry id (a memory location) to a physics actor. + /// + /// + /// Only actors participating in collisions have geometries. + /// + public Dictionary actor_name_map = new Dictionary(); - public IntPtr world; + /// + /// Defects list to remove characters that no longer have finite positions due to some other bug. + /// + /// + /// Used repeatedly in Simulate() but initialized once here. + /// + private readonly List defects = new List(); + + private bool m_NINJA_physics_joints_enabled = false; + //private Dictionary jointpart_name_map = new Dictionary(); + private readonly Dictionary> joints_connecting_actor = new Dictionary>(); + private d.ContactGeom[] contacts; + /// + /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active + /// + private readonly List requestedJointsToBeCreated = new List(); - // split the spaces acording to contents type - // ActiveSpace contains characters and active prims - // StaticSpace contains land and other that is mostly static in enviroment - // this can contain subspaces, like the grid in staticspace - // as now space only contains this 2 top spaces + /// + /// can lock for longer. accessed only by OdeScene. + /// + private readonly List pendingJoints = new List(); - public IntPtr TopSpace; // the global space - public IntPtr ActiveSpace; // space for active prims - public IntPtr StaticSpace; // space for the static things around - public IntPtr GroundSpace; // space for ground + /// + /// can lock for longer. accessed only by OdeScene. + /// + private readonly List activeJoints = new List(); - // some speedup variables - private int spaceGridMaxX; - private int spaceGridMaxY; - private float spacesPerMeter; + /// + /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active + /// + private readonly List requestedJointsToBeDeleted = new List(); + + private Object externalJointRequestsLock = new Object(); + private readonly Dictionary SOPName_to_activeJoint = new Dictionary(); + private readonly Dictionary SOPName_to_pendingJoint = new Dictionary(); + private readonly DoubleDictionary RegionTerrain = new DoubleDictionary(); + private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); + + private d.Contact contact; + private d.Contact TerrainContact; + private d.Contact AvatarMovementprimContact; + private d.Contact AvatarMovementTerrainContact; + private d.Contact WaterContact; + private d.Contact[,] m_materialContacts; + +//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it +//Ckrinke private int m_randomizeWater = 200; + private int m_physicsiterations = 10; + private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag + private readonly PhysicsActor PANull = new NullPhysicsActor(); +// private float step_time = 0.0f; +//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it +//Ckrinke private int ms = 0; + public IntPtr world; + //private bool returncollisions = false; + // private uint obj1LocalID = 0; + private uint obj2LocalID = 0; + //private int ctype = 0; + private OdeCharacter cc1; + private OdePrim cp1; + private OdeCharacter cc2; + private OdePrim cp2; + private int p1ExpectedPoints = 0; + private int p2ExpectedPoints = 0; + //private int cStartStop = 0; + //private string cDictKey = ""; + + public IntPtr space; + + //private IntPtr tmpSpace; + // split static geometry collision handling into spaces of 30 meters + public IntPtr[,] staticPrimspace; - // split static geometry collision into a grid as before - private IntPtr[,] staticPrimspace; - private IntPtr[] staticPrimspaceOffRegion; + /// + /// Used to lock the entire physics scene. Locked during the main part of Simulate() + /// + internal Object OdeLock = new Object(); - public Object OdeLock; - public static Object SimulationLock; + private bool _worldInitialized = false; public IMesher mesher; @@ -322,340 +501,461 @@ namespace OpenSim.Region.Physics.OdePlugin public int physics_logging_interval = 0; public bool physics_logging_append_existing_logfile = false; + + public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f); + public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f); + + // TODO: unused: private uint heightmapWidth = m_regionWidth + 1; + // TODO: unused: private uint heightmapHeight = m_regionHeight + 1; + // TODO: unused: private uint heightmapWidthSamples; + // TODO: unused: private uint heightmapHeightSamples; + + private volatile int m_global_contactcount = 0; + private Vector3 m_worldOffset = Vector3.Zero; public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); private PhysicsScene m_parentScene = null; private ODERayCastRequestManager m_rayCastManager; - public ODEMeshWorker m_meshWorker; -/* maybe needed if ode uses tls - private void checkThread() - { - - int th = Thread.CurrentThread.ManagedThreadId; - if(th != threadid) - { - threadid = th; - d.AllocateODEDataForThread(~0U); - } - } - */ /// /// Initiailizes the scene /// Sets many properties that ODE requires to be stable /// These settings need to be tweaked 'exactly' right or weird stuff happens. /// - public OdeScene(string sceneIdentifier) - { - m_log - = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); - -// checkThread(); - Name = sceneIdentifier; + /// Name of the scene. Useful in debug messages. + public OdeScene(string name) + { + m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + name); - OdeLock = new Object(); - SimulationLock = new Object(); + Name = name; nearCallback = near; - + triCallback = TriCallback; + triArrayCallback = TriArrayCallback; m_rayCastManager = new ODERayCastRequestManager(this); - - - lock (OdeLock) - { - // Create the world and the first space - try - { - world = d.WorldCreate(); - TopSpace = d.HashSpaceCreate(IntPtr.Zero); - // now the major subspaces - ActiveSpace = d.HashSpaceCreate(TopSpace); - StaticSpace = d.HashSpaceCreate(TopSpace); - GroundSpace = d.HashSpaceCreate(TopSpace); - } - catch - { - // i must RtC#FM - } + // Create the world and the first space + world = d.WorldCreate(); + space = d.HashSpaceCreate(IntPtr.Zero); - d.HashSpaceSetLevels(TopSpace, -2, 8); - d.HashSpaceSetLevels(ActiveSpace, -2, 8); - d.HashSpaceSetLevels(StaticSpace, -2, 8); - d.HashSpaceSetLevels(GroundSpace, 0, 8); + contactgroup = d.JointGroupCreate(0); - // demote to second level - d.SpaceSetSublevel(ActiveSpace, 1); - d.SpaceSetSublevel(StaticSpace, 1); - d.SpaceSetSublevel(GroundSpace, 1); + d.WorldSetAutoDisableFlag(world, false); - d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Character | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(ActiveSpace, 0); - d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Land | - CollisionCategories.Water | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(StaticSpace, 0); + #if USE_DRAWSTUFF + Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); + viewthread.Start(); + #endif - d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundSpace, 0); + _watermap = new float[258 * 258]; - contactgroup = d.JointGroupCreate(0); - //contactgroup + // Zero out the prim spaces array (we split our space into smaller spaces so + // we can hit test less. + } - d.WorldSetAutoDisableFlag(world, false); - } +#if USE_DRAWSTUFF + public void startvisualization(object o) + { + ds.Functions fn; + fn.version = ds.VERSION; + fn.start = new ds.CallbackFunction(start); + fn.step = new ds.CallbackFunction(step); + fn.command = new ds.CallbackFunction(command); + fn.stop = null; + fn.path_to_textures = "./textures"; + string[] args = new string[0]; + ds.SimulationLoop(args.Length, args, 352, 288, ref fn); } +#endif // Initialize the mesh plugin -// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region ) public override void Initialise(IMesher meshmerizer, IConfigSource config) { -// checkThread(); + InitializeExtraStats(); + mesher = meshmerizer; m_config = config; + // Defaults - string ode_config = d.GetConfiguration(); - if (ode_config != null && ode_config != "") + if (Environment.OSVersion.Platform == PlatformID.Unix) { - m_log.WarnFormat("ODE configuration: {0}", ode_config); - - if (ode_config.Contains("ODE_Ubit")) - { - OdeUbitLib = true; - } + avPIDD = 3200.0f; + avPIDP = 1400.0f; + avStandupTensor = 2000000f; + } + else + { + avPIDD = 2200.0f; + avPIDP = 900.0f; + avStandupTensor = 550000f; } - - /* - if (region != null) - { - WorldExtents.X = region.RegionSizeX; - WorldExtents.Y = region.RegionSizeY; - } - */ - - // Defaults int contactsPerCollision = 80; - IConfig physicsconfig = null; - if (m_config != null) { - physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; if (physicsconfig != null) { - gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); - gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); - gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); + CollectStats = physicsconfig.GetBoolean("collect_stats", false); + + gravityx = physicsconfig.GetFloat("world_gravityx", 0f); + gravityy = physicsconfig.GetFloat("world_gravityy", 0f); + gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f); + + float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f); + AvatarTerminalVelocity = Util.Clamp(avatarTerminalVelocity, 0, 255f); + if (AvatarTerminalVelocity != avatarTerminalVelocity) + { + m_log.WarnFormat( + "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid. Clamping to {1}", + avatarTerminalVelocity, AvatarTerminalVelocity); + } + + worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4); + worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128); + + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); + smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4); + smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66); - metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); + contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f); - contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); + nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f); + nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f); + nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f); + + mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f); + mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f); + mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f); + + nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f); + nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f); + + mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f); + mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f); ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); - m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations); + m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); - avDensity = physicsconfig.GetFloat("av_density", avDensity); - avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); - avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); - avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius); + avDensity = physicsconfig.GetFloat("av_density", 80f); +// avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); + avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f); + IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false); - contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); - geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); + geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5); geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); -// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); + geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); + + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); + bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); - geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); - bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); + bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f); + bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f); + + forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); + meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); + meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f); + MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f); + m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f); + avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f); + bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f); + } + else + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); + avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f); + bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f); + } physics_logging = physicsconfig.GetBoolean("physics_logging", false); physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); - minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); - maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); + m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false); + minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); } } - m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); - - HalfOdeStep = ODE_STEPSIZE * 0.5f; - odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); + contacts = new d.ContactGeom[contactsPerCollision]; + + staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)]; + + // Centeral contact friction and bounce + // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why + // an avatar falls through in Z but not in X or Y when walking on a prim. + contact.surface.mode |= d.ContactFlags.SoftERP; + contact.surface.mu = nmAvatarObjectContactFriction; + contact.surface.bounce = nmAvatarObjectContactBounce; + contact.surface.soft_cfm = 0.010f; + contact.surface.soft_erp = 0.010f; + + // Terrain contact friction and Bounce + // This is the *non* moving version. Use this when an avatar + // isn't moving to keep it in place better + TerrainContact.surface.mode |= d.ContactFlags.SoftERP; + TerrainContact.surface.mu = nmTerrainContactFriction; + TerrainContact.surface.bounce = nmTerrainContactBounce; + TerrainContact.surface.soft_erp = nmTerrainContactERP; + + WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM); + WaterContact.surface.mu = 0f; // No friction + WaterContact.surface.bounce = 0.0f; // No bounce + WaterContact.surface.soft_cfm = 0.010f; + WaterContact.surface.soft_erp = 0.010f; + + // Prim contact friction and bounce + // THis is the *non* moving version of friction and bounce + // Use this when an avatar comes in contact with a prim + // and is moving + AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction; + AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce; + + // Terrain contact friction bounce and various error correcting calculations + // Use this when an avatar is in contact with the terrain and moving. + AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP; + AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction; + AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce; + AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP; - ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); - GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); - - m_materialContactsData[(int)Material.Stone].mu = 0.8f; - m_materialContactsData[(int)Material.Stone].bounce = 0.4f; - - m_materialContactsData[(int)Material.Metal].mu = 0.3f; - m_materialContactsData[(int)Material.Metal].bounce = 0.4f; - - m_materialContactsData[(int)Material.Glass].mu = 0.2f; - m_materialContactsData[(int)Material.Glass].bounce = 0.7f; - - m_materialContactsData[(int)Material.Wood].mu = 0.6f; - m_materialContactsData[(int)Material.Wood].bounce = 0.5f; - - m_materialContactsData[(int)Material.Flesh].mu = 0.9f; - m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; + /* + + Stone = 0, + /// + Metal = 1, + /// + Glass = 2, + /// + Wood = 3, + /// + Flesh = 4, + /// + Plastic = 5, + /// + Rubber = 6 + */ - m_materialContactsData[(int)Material.Plastic].mu = 0.4f; - m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; + m_materialContacts = new d.Contact[7,2]; + + m_materialContacts[(int)Material.Stone, 0] = new d.Contact(); + m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Stone, 1] = new d.Contact(); + m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Metal, 0] = new d.Contact(); + m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Metal, 1] = new d.Contact(); + m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Glass, 0] = new d.Contact(); + m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f; + m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f; + m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f; - m_materialContactsData[(int)Material.Rubber].mu = 0.9f; - m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; + /* + private float nmAvatarObjectContactFriction = 250f; + private float nmAvatarObjectContactBounce = 0.1f; - m_materialContactsData[(int)Material.light].mu = 0.0f; - m_materialContactsData[(int)Material.light].bounce = 0.0f; + private float mAvatarObjectContactFriction = 75f; + private float mAvatarObjectContactBounce = 0.1f; + */ + m_materialContacts[(int)Material.Glass, 1] = new d.Contact(); + m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f; + m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f; + m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Wood, 0] = new d.Contact(); + m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Wood, 1] = new d.Contact(); + m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Flesh, 0] = new d.Contact(); + m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Flesh, 1] = new d.Contact(); + m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Plastic, 0] = new d.Contact(); + m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Plastic, 1] = new d.Contact(); + m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Rubber, 0] = new d.Contact(); + m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction; + m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce; + m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f; + + m_materialContacts[(int)Material.Rubber, 1] = new d.Contact(); + m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP; + m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction; + m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce; + m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f; + m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f; + + d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh); // Set the gravity,, don't disable things automatically (we set it explicitly on some things) d.WorldSetGravity(world, gravityx, gravityy, gravityz); d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); - d.WorldSetLinearDamping(world, 0.002f); - d.WorldSetAngularDamping(world, 0.002f); - d.WorldSetAngularDampingThreshold(world, 0f); - d.WorldSetLinearDampingThreshold(world, 0f); - d.WorldSetMaxAngularSpeed(world, 100f); - - d.WorldSetCFM(world,1e-6f); // a bit harder than default - //d.WorldSetCFM(world, 1e-4f); // a bit harder than default - d.WorldSetERP(world, 0.6f); // higher than original + d.WorldSetLinearDamping(world, 256f); + d.WorldSetAngularDamping(world, 256f); + d.WorldSetAngularDampingThreshold(world, 256f); + d.WorldSetLinearDampingThreshold(world, 256f); + d.WorldSetMaxAngularSpeed(world, 256f); // Set how many steps we go without running collision testing // This is in addition to the step size. // Essentially Steps * m_physicsiterations d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + //d.WorldSetContactMaxCorrectingVel(world, 1000.0f); - d.WorldSetContactMaxCorrectingVel(world, 60.0f); - - spacesPerMeter = 1 / metersInSpace; - spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); - spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter); - - staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; - - // create all spaces now - int i, j; - IntPtr newspace; - - for (i = 0; i < spaceGridMaxX; i++) - for (j = 0; j < spaceGridMaxY; j++) - { - newspace = d.HashSpaceCreate(StaticSpace); - d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); - waitForSpaceUnlock(newspace); - d.SpaceSetSublevel(newspace, 2); - d.HashSpaceSetLevels(newspace, -2, 8); - d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Land | - CollisionCategories.Water | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(newspace, 0); - - staticPrimspace[i, j] = newspace; - } - // let this now be real maximum values - spaceGridMaxX--; - spaceGridMaxY--; - - // create 4 off world spaces (x<0,x>max,y<0,y>max) - staticPrimspaceOffRegion = new IntPtr[4]; - - for (i = 0; i < 4; i++) + for (int i = 0; i < staticPrimspace.GetLength(0); i++) + { + for (int j = 0; j < staticPrimspace.GetLength(1); j++) { - newspace = d.HashSpaceCreate(StaticSpace); - d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); - waitForSpaceUnlock(newspace); - d.SpaceSetSublevel(newspace, 2); - d.HashSpaceSetLevels(newspace, -2, 8); - d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Land | - CollisionCategories.Water | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(newspace, 0); - - staticPrimspaceOffRegion[i] = newspace; + staticPrimspace[i, j] = IntPtr.Zero; } + } - m_lastframe = DateTime.UtcNow; - m_lastMeshExpire = m_lastframe; + _worldInitialized = true; } - internal void waitForSpaceUnlock(IntPtr space) - { - //if (space != IntPtr.Zero) - //while (d.SpaceLockQuery(space)) { } // Wait and do nothing - } +// internal void waitForSpaceUnlock(IntPtr space) +// { +// //if (space != IntPtr.Zero) +// //while (d.SpaceLockQuery(space)) { } // Wait and do nothing +// } + +// /// +// /// Debug space message for printing the space that a prim/avatar is in. +// /// +// /// +// /// Returns which split up space the given position is in. +// public string whichspaceamIin(Vector3 pos) +// { +// return calculateSpaceForGeom(pos).ToString(); +// } #region Collision Detection - // sets a global contact for a joint for contactgeom , and base contact description) - - private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale) + /// + /// Collides two geometries. + /// + /// + /// + /// /param> + /// + /// + /// + private int CollideGeoms( + IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize) { - if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) - return IntPtr.Zero; + int count; - float erp = contactGeom.depth; - erp *= erpscale; - if (erp < minERP) - erp = minERP; - else if (erp > MaxERP) - erp = MaxERP; - - float depth = contactGeom.depth * dscale; - if (depth > 0.5f) - depth = 0.5f; + lock (OdeScene.UniversalColliderSyncObject) + { + // We do this inside the lock so that we don't count any delay in acquiring it + if (CollectStats) + m_nativeCollisionStartTick = Util.EnvironmentTickCount(); - d.Contact newcontact = new d.Contact(); - newcontact.geom.depth = depth; - newcontact.geom.g1 = contactGeom.g1; - newcontact.geom.g2 = contactGeom.g2; - newcontact.geom.pos = contactGeom.pos; - newcontact.geom.normal = contactGeom.normal; - newcontact.geom.side1 = contactGeom.side1; - newcontact.geom.side2 = contactGeom.side2; + count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize); + } - // this needs bounce also - newcontact.surface.mode = comumContactFlags; - newcontact.surface.mu = mu; - newcontact.surface.bounce = bounce; - newcontact.surface.soft_cfm = cfm; - newcontact.surface.soft_erp = erp; + // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably + // negligable + if (CollectStats) + m_stats[ODENativeGeomCollisionFrameMsStatName] + += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); - IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); - Marshal.StructureToPtr(newcontact, contact, true); - return d.JointCreateContactPtr(world, contactgroup, contact); + return count; } - private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + /// + /// Collide two spaces or a space and a geometry. + /// + /// + /// /param> + /// + private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data) { - if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) - return false; + if (CollectStats) + { + m_inCollisionTiming = true; + m_nativeCollisionStartTick = Util.EnvironmentTickCount(); + } - IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); - newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); - return true; + d.SpaceCollide2(space1, space2, data, nearCallback); + + if (CollectStats && m_inCollisionTiming) + { + m_stats[ODENativeSpaceCollisionFrameMsStatName] + += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); + m_inCollisionTiming = false; + } } /// @@ -664,50 +964,76 @@ namespace OpenSim.Region.Physics.OdePlugin /// The space that contains the geoms. Remember, spaces are also geoms /// a geometry or space /// another geometry or space - /// - private void near(IntPtr space, IntPtr g1, IntPtr g2) { - // no lock here! It's invoked from within Simulate(), which is thread-locked + if (CollectStats && m_inCollisionTiming) + { + m_stats[ODENativeSpaceCollisionFrameMsStatName] + += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); + m_inCollisionTiming = false; + } - if (m_global_contactcount >= maxContactsbeforedeath) - return; +// m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space); + // no lock here! It's invoked from within Simulate(), which is thread-locked // Test if we're colliding a geom with a space. // If so we have to drill down into the space recursively - if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) - return; - if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) { + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + // Separating static prim geometry spaces. // We'll be calling near recursivly if one // of them is a space to find all of the // contact points in the space try { - d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + CollideSpaces(g1, g2, IntPtr.Zero); } catch (AccessViolationException) { - m_log.Warn("[PHYSICS]: Unable to collide test a space"); + m_log.Error("[ODE SCENE]: Unable to collide test a space"); return; } - //here one should check collisions of geoms inside a space - // but on each space we only should have geoms that not colide amoung each other - // so we don't dig inside spaces + //Colliding a space or a geom with a space or a geom. so drill down + + //Collide all geoms in each space.. + //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback); + //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback); return; } - // get geom bodies to check if we already a joint contact - // guess this shouldn't happen now + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + IntPtr b1 = d.GeomGetBody(g1); IntPtr b2 = d.GeomGetBody(g2); // d.GeomClassID id = d.GeomGetClass(g1); + String name1 = null; + String name2 = null; + + if (!geom_name_map.TryGetValue(g1, out name1)) + { + name1 = "null"; + } + if (!geom_name_map.TryGetValue(g2, out name2)) + { + name2 = "null"; + } + + //if (id == d.GeomClassId.TriMeshClass) + //{ + // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2); + //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); + //} + // Figure out how many contact points we have int count = 0; + try { // Colliding Geom To Geom @@ -719,611 +1045,914 @@ namespace OpenSim.Region.Physics.OdePlugin if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; -// debug - PhysicsActor dp2; - if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) - { - d.AABB aabb; - d.GeomGetAABB(g2, out aabb); - float x = aabb.MaxX - aabb.MinX; - float y = aabb.MaxY - aabb.MinY; - float z = aabb.MaxZ - aabb.MinZ; - if (x > 60.0f || y > 60.0f || z > 60.0f) - { - if (!actor_name_map.TryGetValue(g2, out dp2)) - m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); - else - m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", - dp2.Name, dp2.Size, x, y, z, - dp2.Position.ToString(), - dp2.Orientation.ToString(), - dp2.Orientation.Length()); - return; - } - } -// - + count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf); + // All code after this is only relevant if we have any collisions + if (count <= 0) + return; - if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || - d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) - { - int cflags; - unchecked - { - cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); - } - count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); - } - else - count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + if (count > contacts.Length) + m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length); } 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); + m_log.Error( + "[ODE SCENE]: 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."); base.TriggerPhysicsBasedRestart(); } catch (Exception e) { - m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); + m_log.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message); return; } - // contacts done - if (count == 0) - return; - - // try get physical actors PhysicsActor p1; PhysicsActor p2; - + + p1ExpectedPoints = 0; + p2ExpectedPoints = 0; + if (!actor_name_map.TryGetValue(g1, out p1)) { - m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); - return; + p1 = PANull; } if (!actor_name_map.TryGetValue(g2, out p2)) { - m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); - return; + p2 = PANull; } - // update actors collision score - if (p1.CollisionScore >= float.MaxValue - count) + ContactPoint maxDepthContact = new ContactPoint(); + if (p1.CollisionScore + count >= float.MaxValue) p1.CollisionScore = 0; p1.CollisionScore += count; - if (p2.CollisionScore >= float.MaxValue - count) + if (p2.CollisionScore + count >= float.MaxValue) p2.CollisionScore = 0; p2.CollisionScore += count; - // get first contact - d.ContactGeom curContact = new d.ContactGeom(); - if (!GetCurContactGeom(0, ref curContact)) - return; - // for now it's the one with max depth - ContactPoint maxDepthContact = new ContactPoint( + for (int i = 0; i < count; i++) + { + d.ContactGeom curContact = contacts[i]; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact = new ContactPoint( new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), curContact.depth - ); - // do volume detection case - if ( - (p1.IsVolumeDtc || p2.IsVolumeDtc)) - { - collision_accounting_events(p1, p2, maxDepthContact); - return; - } + ); + } - // big messy collision analises + //m_log.Warn("[CCOUNT]: " + count); + IntPtr joint; + // If we're colliding with terrain, use 'TerrainContact' instead of contact. + // allows us to have different settings + + // We only need to test p2 for 'jump crouch purposes' + if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim) + { + // Testing if the collision is at the feet of the avatar + + //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); + if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)) + p2.IsColliding = true; + } + else + { + p2.IsColliding = true; + } + + //if ((framecount % m_returncollisions) == 0) - Vector3 normoverride = Vector3.Zero; //damm c# + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Agent: + p1ExpectedPoints = avatarExpectedContacts; + p2.CollidingObj = true; + break; + case (int)ActorTypes.Prim: + if (p1 != null && p1 is OdePrim) + p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts; - float mu = 0; - float bounce = 0; - float cfm = 0.0001f; - float erpscale = 1.0f; - float dscale = 1.0f; - bool IgnoreNegSides = false; + if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + break; + case (int)ActorTypes.Unknown: + p2.CollidingGround = true; + break; + default: + p2.CollidingGround = true; + break; + } - ContactData contactdata1 = new ContactData(0, 0, false); - ContactData contactdata2 = new ContactData(0, 0, false); + // we don't want prim or avatar to explode - bool dop1foot = false; - bool dop2foot = false; - bool ignore = false; - bool AvanormOverride = false; + #region InterPenetration Handling - Unintended physics explosions +# region disabled code1 - switch (p1.PhysicsActorType) - { - case (int)ActorTypes.Agent: + if (curContact.depth >= 0.08f) + { + //This is disabled at the moment only because it needs more tweaking + //It will eventually be uncommented + /* + if (contact.depth >= 1.00f) { - AvanormOverride = true; - Vector3 tmp = p2.Position - p1.Position; - normoverride = p2.Velocity - p1.Velocity; - mu = normoverride.LengthSquared(); + //m_log.Debug("[PHYSICS]: " + contact.depth.ToString()); + } - if (mu > 1e-6) + //If you interpenetrate a prim with an agent + if ((p2.PhysicsActorType == (int) ActorTypes.Agent && + p1.PhysicsActorType == (int) ActorTypes.Prim) || + (p1.PhysicsActorType == (int) ActorTypes.Agent && + p2.PhysicsActorType == (int) ActorTypes.Prim)) + { + + //contact.depth = contact.depth * 4.15f; + /* + if (p2.PhysicsActorType == (int) ActorTypes.Agent) { - mu = 1.0f / (float)Math.Sqrt(mu); - normoverride *= mu; - mu = Vector3.Dot(tmp, normoverride); - if (mu > 0) - normoverride *= -1; + p2.CollidingObj = true; + contact.depth = 0.003f; + p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f); + OdeCharacter character = (OdeCharacter) p2; + character.SetPidStatus(true); + contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2)); + } else { - tmp.Normalize(); - normoverride = -tmp; - } - switch (p2.PhysicsActorType) + //contact.depth = 0.0000000f; + } + if (p1.PhysicsActorType == (int) ActorTypes.Agent) { - case (int)ActorTypes.Agent: - p1.CollidingObj = true; - p2.CollidingObj = true; - break; - case (int)ActorTypes.Prim: - if (p2.Velocity.LengthSquared() > 0.0f) - p2.CollidingObj = true; - dop1foot = true; - break; + p1.CollidingObj = true; + contact.depth = 0.003f; + p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f); + contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2)); + OdeCharacter character = (OdeCharacter)p1; + character.SetPidStatus(true); + } + else + { - default: - ignore = true; // avatar to terrain and water ignored - break; + //contact.depth = 0.0000000f; } - break; + + + } - - case (int)ActorTypes.Prim: - switch (p2.PhysicsActorType) +*/ + // If you interpenetrate a prim with another prim + /* + if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim) { - case (int)ActorTypes.Agent: - AvanormOverride = true; - - Vector3 tmp = p2.Position - p1.Position; - normoverride = p2.Velocity - p1.Velocity; - mu = normoverride.LengthSquared(); - if (mu > 1e-6) - { - mu = 1.0f / (float)Math.Sqrt(mu); - normoverride *= mu; - mu = Vector3.Dot(tmp, normoverride); - if (mu > 0) - normoverride *= -1; - } - else - { - tmp.Normalize(); - normoverride = -tmp; - } - - bounce = 0; - mu = 0; - cfm = 0.0001f; - - dop2foot = true; - if (p1.Velocity.LengthSquared() > 0.0f) - p1.CollidingObj = true; - break; - - case (int)ActorTypes.Prim: - if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) + #region disabledcode2 + //OdePrim op1 = (OdePrim)p1; + //OdePrim op2 = (OdePrim)p2; + //op1.m_collisionscore++; + //op2.m_collisionscore++; + + //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000) + //{ + //op1.m_taintdisable = true; + //AddPhysicsActorTaint(p1); + //op2.m_taintdisable = true; + //AddPhysicsActorTaint(p2); + //} + + //if (contact.depth >= 0.25f) + //{ + // Don't collide, one or both prim will expld. + + //op1.m_interpenetrationcount++; + //op2.m_interpenetrationcount++; + //interpenetrations_before_disable = 200; + //if (op1.m_interpenetrationcount >= interpenetrations_before_disable) + //{ + //op1.m_taintdisable = true; + //AddPhysicsActorTaint(p1); + //} + //if (op2.m_interpenetrationcount >= interpenetrations_before_disable) + //{ + // op2.m_taintdisable = true; + //AddPhysicsActorTaint(p2); + //} + + //contact.depth = contact.depth / 8f; + //contact.normal = new d.Vector3(0, 0, 1); + //} + //if (op1.m_disabled || op2.m_disabled) + //{ + //Manually disabled objects stay disabled + //contact.depth = 0f; + //} + #endregion + } + */ +#endregion + if (curContact.depth >= 1.00f) + { + //m_log.Info("[P]: " + contact.depth.ToString()); + if ((p2.PhysicsActorType == (int) ActorTypes.Agent && + p1.PhysicsActorType == (int) ActorTypes.Unknown) || + (p1.PhysicsActorType == (int) ActorTypes.Agent && + p2.PhysicsActorType == (int) ActorTypes.Unknown)) + { + if (p2.PhysicsActorType == (int) ActorTypes.Agent) { - p1.CollidingObj = true; - p2.CollidingObj = true; + if (p2 is OdeCharacter) + { + OdeCharacter character = (OdeCharacter) p2; + + //p2.CollidingObj = true; + curContact.depth = 0.00000003f; + p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f); + curContact.pos = + new d.Vector3(curContact.pos.X + (p1.Size.X/2), + curContact.pos.Y + (p1.Size.Y/2), + curContact.pos.Z + (p1.Size.Z/2)); + character.SetPidStatus(true); + } } - p1.getContactData(ref contactdata1); - p2.getContactData(ref contactdata2); - bounce = contactdata1.bounce * contactdata2.bounce; - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - - cfm = p1.Mass; - if (cfm > p2.Mass) - cfm = p2.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - if (dscale > 1.0f) - dscale = 1.0f; - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; - else if (cfm < 0.00001f) - cfm = 0.00001f; - - if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) - mu *= frictionMovementMult; - - break; - case (int)ActorTypes.Ground: - p1.getContactData(ref contactdata1); - bounce = contactdata1.bounce * TerrainBounce; - mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); - if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) - mu *= frictionMovementMult; - p1.CollidingGround = true; - - cfm = p1.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - if (dscale > 1.0f) - dscale = 1.0f; - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; - else if (cfm < 0.00001f) - cfm = 0.00001f; - - if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) + if (p1.PhysicsActorType == (int) ActorTypes.Agent) { - if (curContact.side1 > 0) - IgnoreNegSides = true; + if (p1 is OdeCharacter) + { + OdeCharacter character = (OdeCharacter) p1; + + //p2.CollidingObj = true; + curContact.depth = 0.00000003f; + p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f); + curContact.pos = + new d.Vector3(curContact.pos.X + (p1.Size.X/2), + curContact.pos.Y + (p1.Size.Y/2), + curContact.pos.Z + (p1.Size.Z/2)); + character.SetPidStatus(true); + } } - break; - - case (int)ActorTypes.Water: - default: - ignore = true; - break; + } } - break; - - case (int)ActorTypes.Ground: - if (p2.PhysicsActorType == (int)ActorTypes.Prim) - { - p2.CollidingGround = true; - p2.getContactData(ref contactdata2); - bounce = contactdata2.bounce * TerrainBounce; - mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); - - cfm = p2.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - - if (dscale > 1.0f) - dscale = 1.0f; - - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; - else if (cfm < 0.00001f) - cfm = 0.00001f; + } - if (curContact.side1 > 0) // should be 2 ? - IgnoreNegSides = true; + #endregion - if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) - mu *= frictionMovementMult; - } - else - ignore = true; - break; + // Logic for collision handling + // Note, that if *all* contacts are skipped (VolumeDetect) + // The prim still detects (and forwards) collision events but + // appears to be phantom for the world + Boolean skipThisContact = false; - case (int)ActorTypes.Water: - default: - break; - } - if (ignore) - return; + if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect)) + skipThisContact = true; // No collision on volume detect prims - IntPtr Joint; + if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) + skipThisContact = true; // No collision on volume detect prims - int i = 0; - while(true) - { + if (!skipThisContact && curContact.depth < 0f) + skipThisContact = true; - if (IgnoreNegSides && curContact.side1 < 0) - { - if (++i >= count) - break; + if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType)) + skipThisContact = true; - if (!GetCurContactGeom(i, ref curContact)) - break; - } - else + const int maxContactsbeforedeath = 4000; + joint = IntPtr.Zero; + if (!skipThisContact) { + _perloopContact.Add(curContact); - if (AvanormOverride) + if (name1 == "Terrain" || name2 == "Terrain") { - if (curContact.depth > 0.3f) + if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && + (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) { - if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) - p1.IsColliding = true; - if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) - p2.IsColliding = true; - curContact.normal.X = normoverride.X; - curContact.normal.Y = normoverride.Y; - curContact.normal.Z = normoverride.Z; - } + p2ExpectedPoints = avatarExpectedContacts; + // Avatar is moving on terrain, use the movement terrain contact + AvatarMovementTerrainContact.geom = curContact; + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact); + m_global_contactcount++; + } + } else { - if (dop1foot) + if (p2.PhysicsActorType == (int)ActorTypes.Agent) + { + p2ExpectedPoints = avatarExpectedContacts; + // Avatar is standing on terrain, use the non moving terrain contact + TerrainContact.geom = curContact; + + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref TerrainContact); + m_global_contactcount++; + } + } + else { - float sz = p1.Size.Z; - Vector3 vtmp = p1.Position; - float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; - if (ppos > 0f) + if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim) { - if (!p1.Flying) + // prim prim contact + // int pj294950 = 0; + int movintYN = 0; + int material = (int) Material.Wood; + // prim terrain contact + if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) { - d.AABB aabb; - d.GeomGetAABB(g2, out aabb); - float tmp = vtmp.Z - sz * .18f; + movintYN = 1; + } - if (aabb.MaxZ < tmp) - { - vtmp.X = curContact.pos.X - vtmp.X; - vtmp.Y = curContact.pos.Y - vtmp.Y; - vtmp.Z = -0.2f; - vtmp.Normalize(); - curContact.normal.X = vtmp.X; - curContact.normal.Y = vtmp.Y; - curContact.normal.Z = vtmp.Z; - } + if (p2 is OdePrim) + { + material = ((OdePrim) p2).m_material; + p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; + } + + // Unnessesary because p1 is defined above + //if (p1 is OdePrim) + // { + // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts; + // } + //m_log.DebugFormat("Material: {0}", material); + + m_materialContacts[material, movintYN].geom = curContact; + + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); + m_global_contactcount++; } } else - p1.IsColliding = true; + { + int movintYN = 0; + // prim terrain contact + if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) + { + movintYN = 1; + } - } + int material = (int)Material.Wood; - if (dop2foot) - { - float sz = p2.Size.Z; - Vector3 vtmp = p2.Position; - float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; - if (ppos > 0f) - { - if (!p2.Flying) + if (p2 is OdePrim) { - d.AABB aabb; - d.GeomGetAABB(g1, out aabb); - float tmp = vtmp.Z - sz * .18f; + material = ((OdePrim)p2).m_material; + p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; + } - if (aabb.MaxZ < tmp) - { - vtmp.X = curContact.pos.X - vtmp.X; - vtmp.Y = curContact.pos.Y - vtmp.Y; - vtmp.Z = -0.2f; - vtmp.Normalize(); - curContact.normal.X = vtmp.X; - curContact.normal.Y = vtmp.Y; - curContact.normal.Z = vtmp.Z; - } + //m_log.DebugFormat("Material: {0}", material); + m_materialContacts[material, movintYN].geom = curContact; + + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); + m_global_contactcount++; } } - else - p2.IsColliding = true; - } } + //if (p2.PhysicsActorType == (int)ActorTypes.Prim) + //{ + //m_log.Debug("[PHYSICS]: prim contacting with ground"); + //} + } + else if (name1 == "Water" || name2 == "Water") + { + /* + if ((p2.PhysicsActorType == (int) ActorTypes.Prim)) + { + } + else + { + } + */ + //WaterContact.surface.soft_cfm = 0.0000f; + //WaterContact.surface.soft_erp = 0.00000f; + if (curContact.depth > 0.1f) + { + curContact.depth *= 52; + //contact.normal = new d.Vector3(0, 0, 1); + //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f); + } + + WaterContact.geom = curContact; + + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref WaterContact); + m_global_contactcount++; + } + //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth); } + else + { + if ((p2.PhysicsActorType == (int)ActorTypes.Agent)) + { + p2ExpectedPoints = avatarExpectedContacts; + if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) + { + // Avatar is moving on a prim, use the Movement prim contact + AvatarMovementprimContact.geom = curContact; - Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); - d.JointAttach(Joint, b1, b2); + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact); + m_global_contactcount++; + } + } + else + { + // Avatar is standing still on a prim, use the non movement contact + contact.geom = curContact; - if (++m_global_contactcount >= maxContactsbeforedeath) - break; + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref contact); + m_global_contactcount++; + } + } + } + else if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + //p1.PhysicsActorType + int material = (int)Material.Wood; - if (++i >= count) - break; + if (p2 is OdePrim) + { + material = ((OdePrim)p2).m_material; + p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; + } + + //m_log.DebugFormat("Material: {0}", material); + m_materialContacts[material, 0].geom = curContact; - if (!GetCurContactGeom(i, ref curContact)) - break; + if (m_global_contactcount < maxContactsbeforedeath) + { + joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]); + m_global_contactcount++; + } + } + } - if (curContact.depth > maxDepthContact.PenetrationDepth) + if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide! { - maxDepthContact.Position.X = curContact.pos.X; - maxDepthContact.Position.Y = curContact.pos.Y; - maxDepthContact.Position.Z = curContact.pos.Z; - maxDepthContact.SurfaceNormal.X = curContact.normal.X; - maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; - maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; - maxDepthContact.PenetrationDepth = curContact.depth; + d.JointAttach(joint, b1, b2); + m_global_contactcount++; } } - } - collision_accounting_events(p1, p2, maxDepthContact); - -/* - if (notskipedcount > geomContactPointsStartthrottle) - { - // If there are more then 3 contact points, it's likely - // that we've got a pile of objects, so ... - // We don't want to send out hundreds of terse updates over and over again - // so lets throttle them and send them again after it's somewhat sorted out. - this needs checking so out for now - if (b1 != IntPtr.Zero) - p1.ThrottleUpdates = true; - if (b2 != IntPtr.Zero) - p2.ThrottleUpdates = true; + collision_accounting_events(p1, p2, maxDepthContact); + if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle)) + { + // If there are more then 3 contact points, it's likely + // that we've got a pile of objects, so ... + // We don't want to send out hundreds of terse updates over and over again + // so lets throttle them and send them again after it's somewhat sorted out. + p2.ThrottleUpdates = true; + } + //m_log.Debug(count.ToString()); + //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); } - */ } - private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) + private bool checkDupe(d.ContactGeom contactGeom, int atype) { - uint obj2LocalID = 0; + if (!m_filterCollisions) + return false; - bool p1events = p1.SubscribedEvents(); - bool p2events = p2.SubscribedEvents(); + bool result = false; - if (p1.IsVolumeDtc) - p2events = false; - if (p2.IsVolumeDtc) - p1events = false; + ActorTypes at = (ActorTypes)atype; - if (!p2events && !p1events) - return; + foreach (d.ContactGeom contact in _perloopContact) + { + //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2)) + //{ + // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2) + if (at == ActorTypes.Agent) + { + if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) + && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) + && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f))) + { + if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f) + { + //contactGeom.depth *= .00005f; + //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); + // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); + result = true; + break; + } +// else +// { +// //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); +// } + } +// else +// { +// //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); +// //int i = 0; +// } + } + else if (at == ActorTypes.Prim) + { + //d.AABB aabb1 = new d.AABB(); + //d.AABB aabb2 = new d.AABB(); - Vector3 vel = Vector3.Zero; - if (p2 != null && p2.IsPhysical) - vel = p2.Velocity; + //d.GeomGetAABB(contactGeom.g2, out aabb2); + //d.GeomGetAABB(contactGeom.g1, out aabb1); + //aabb1. + if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f))) + { + if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z) + { + if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f) + { + result = true; + break; + } + } + //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); + //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); + } + } + } - if (p1 != null && p1.IsPhysical) - vel -= p1.Velocity; + return result; + } - contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); + private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) + { + // obj1LocalID = 0; + //returncollisions = false; + obj2LocalID = 0; + //ctype = 0; + //cStartStop = 0; + if (!p2.SubscribedEvents() && !p1.SubscribedEvents()) + return; - switch ((ActorTypes)p1.PhysicsActorType) + switch ((ActorTypes)p2.PhysicsActorType) { case ActorTypes.Agent: + cc2 = (OdeCharacter)p2; + + // obj1LocalID = cc2.m_localID; + switch ((ActorTypes)p1.PhysicsActorType) + { + case ActorTypes.Agent: + cc1 = (OdeCharacter)p1; + obj2LocalID = cc1.LocalID; + cc1.AddCollisionEvent(cc2.LocalID, contact); + //ctype = (int)CollisionCategories.Character; + + //if (cc1.CollidingObj) + //cStartStop = (int)StatusIndicators.Generic; + //else + //cStartStop = (int)StatusIndicators.Start; + + //returncollisions = true; + break; + + case ActorTypes.Prim: + if (p1 is OdePrim) + { + cp1 = (OdePrim) p1; + obj2LocalID = cp1.LocalID; + cp1.AddCollisionEvent(cc2.LocalID, contact); + } + //ctype = (int)CollisionCategories.Geom; + + //if (cp1.CollidingObj) + //cStartStop = (int)StatusIndicators.Generic; + //else + //cStartStop = (int)StatusIndicators.Start; + + //returncollisions = true; + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + obj2LocalID = 0; + //ctype = (int)CollisionCategories.Land; + //returncollisions = true; + break; + } + + cc2.AddCollisionEvent(obj2LocalID, contact); + break; + case ActorTypes.Prim: + + if (p2 is OdePrim) { - switch ((ActorTypes)p2.PhysicsActorType) + cp2 = (OdePrim) p2; + + // obj1LocalID = cp2.m_localID; + switch ((ActorTypes) p1.PhysicsActorType) { case ActorTypes.Agent: + if (p1 is OdeCharacter) + { + cc1 = (OdeCharacter) p1; + obj2LocalID = cc1.LocalID; + cc1.AddCollisionEvent(cp2.LocalID, contact); + //ctype = (int)CollisionCategories.Character; + + //if (cc1.CollidingObj) + //cStartStop = (int)StatusIndicators.Generic; + //else + //cStartStop = (int)StatusIndicators.Start; + //returncollisions = true; + } + break; case ActorTypes.Prim: - if (p2events) + + if (p1 is OdePrim) { - AddCollisionEventReporting(p2); - p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); + cp1 = (OdePrim) p1; + obj2LocalID = cp1.LocalID; + cp1.AddCollisionEvent(cp2.LocalID, contact); + //ctype = (int)CollisionCategories.Geom; + + //if (cp1.CollidingObj) + //cStartStop = (int)StatusIndicators.Generic; + //else + //cStartStop = (int)StatusIndicators.Start; + + //returncollisions = true; } - obj2LocalID = p2.ParentActor.LocalID; break; case ActorTypes.Ground: case ActorTypes.Unknown: - default: obj2LocalID = 0; + //ctype = (int)CollisionCategories.Land; + + //returncollisions = true; break; } - if (p1events) - { - contact.SurfaceNormal = -contact.SurfaceNormal; - AddCollisionEventReporting(p1); - p1.AddCollisionEvent(obj2LocalID, contact); - } - break; - } - case ActorTypes.Ground: - case ActorTypes.Unknown: - default: - { - if (p2events && !p2.IsVolumeDtc) - { - AddCollisionEventReporting(p2); - p2.AddCollisionEvent(0, contact); - } - break; + + cp2.AddCollisionEvent(obj2LocalID, contact); } + break; } + //if (returncollisions) + //{ + + //lock (m_storedCollisions) + //{ + //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString(); + //if (m_storedCollisions.ContainsKey(cDictKey)) + //{ + //sCollisionData objd = m_storedCollisions[cDictKey]; + //objd.NumberOfCollisions += 1; + //objd.lastframe = framecount; + //m_storedCollisions[cDictKey] = objd; + //} + //else + //{ + //sCollisionData objd = new sCollisionData(); + //objd.ColliderLocalId = obj1LocalID; + //objd.CollidedWithLocalId = obj2LocalID; + //objd.CollisionType = ctype; + //objd.NumberOfCollisions = 1; + //objd.lastframe = framecount; + //objd.StatusIndicator = cStartStop; + //m_storedCollisions.Add(cDictKey, objd); + //} + //} + // } + } + + private int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount) + { + /* String name1 = null; + String name2 = null; + + if (!geom_name_map.TryGetValue(trimesh, out name1)) + { + name1 = "null"; + } + if (!geom_name_map.TryGetValue(refObject, out name2)) + { + name2 = "null"; + } + + m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2); + */ + return 1; + } + + private int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex) + { +// String name1 = null; +// String name2 = null; +// +// if (!geom_name_map.TryGetValue(trimesh, out name1)) +// { +// name1 = "null"; +// } +// +// if (!geom_name_map.TryGetValue(refObject, out name2)) +// { +// name2 = "null"; +// } + + // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex); + + d.Vector3 v0 = new d.Vector3(); + d.Vector3 v1 = new d.Vector3(); + d.Vector3 v2 = new d.Vector3(); + + d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2); + // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z); + + return 1; } /// /// This is our collision testing routine in ODE /// - /// private void collision_optimized() { - lock (_characters) - { + _perloopContact.Clear(); + + foreach (OdeCharacter chr in _characters) + { + // Reset the collision values to false + // since we don't know if we're colliding yet + if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) + continue; + + chr.IsColliding = false; + chr.CollidingGround = false; + chr.CollidingObj = false; + + // Test the avatar's geometry for collision with the space + // This will return near and the space that they are the closest to + // And we'll run this again against the avatar and the space segment + // This will return with a bunch of possible objects in the space segment + // and we'll run it again on all of them. try { - foreach (OdeCharacter chr in _characters) - { - if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) - continue; - - chr.IsColliding = false; - // chr.CollidingGround = false; not done here - chr.CollidingObj = false; - // do colisions with static space - d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback); - // no coll with gnd - } + CollideSpaces(space, chr.Shell, IntPtr.Zero); } catch (AccessViolationException) { - m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); + m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", Name); } - + + //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y); + //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10) + //{ + //chr.Position.Z = terrainheight + 10.0f; + //forcedZ = true; + //} } - lock (_activeprims) + if (CollectStats) { - foreach (OdePrim aprim in _activeprims) - { - aprim.CollisionScore = 0; - aprim.IsColliding = false; - } + m_tempAvatarCollisionsThisFrame = _perloopContact.Count; + m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame; } - // collide active prims with static enviroment - lock (_activegroups) + List removeprims = null; + foreach (OdePrim chr in _activeprims) { - try + if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled)) { - foreach (OdePrim prm in _activegroups) + try { - if (!prm.m_outbounds) + lock (chr) { - if (d.BodyIsEnabled(prm.Body)) + if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false) + { + CollideSpaces(space, chr.prim_geom, IntPtr.Zero); + } + else { - d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); - d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); + if (removeprims == null) + { + removeprims = new List(); + } + removeprims.Add(chr); + m_log.Error( + "[ODE SCENE]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!"); } } } + catch (AccessViolationException) + { + m_log.Error("[ODE SCENE]: Unable to space collide"); + } } - catch (AccessViolationException) + } + + if (CollectStats) + m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame; + + if (removeprims != null) + { + foreach (OdePrim chr in removeprims) { - m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); + _activeprims.Remove(chr); } } - // finally colide active things amoung them - try + } + + #endregion + + public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) + { + m_worldOffset = offset; + WorldExtents = new Vector2(extents.X, extents.Y); + m_parentScene = pScene; + } + + // Recovered for use by fly height. Kitto Flora + internal float GetTerrainHeightAtXY(float x, float y) + { + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + + IntPtr heightFieldGeom = IntPtr.Zero; + + if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom)) { - d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); + if (heightFieldGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + { + + int index; + + + if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y || + (int)x < 0.001f || (int)y < 0.001f) + return 0; + + x = x - offsetX; + y = y - offsetY; + + index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y); + + if (index < TerrainHeightFieldHeights[heightFieldGeom].Length) + { + //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]); + return (float)TerrainHeightFieldHeights[heightFieldGeom][index]; + } + + else + return 0f; + } + else + { + return 0f; + } + + } + else + { + return 0f; + } + } - catch (AccessViolationException) + else { - m_log.Warn("[PHYSICS]: Unable to collide in Active space"); + return 0f; } -// _perloopContact.Clear(); - } + } +// End recovered. Kitto Flora - #endregion /// /// Add actor to the list that should receive collision events in the simulate loop. /// /// - public void AddCollisionEventReporting(PhysicsActor obj) + internal void AddCollisionEventReporting(PhysicsActor obj) { - if (!_collisionEventPrim.Contains(obj)) - _collisionEventPrim.Add(obj); +// m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID); + + lock (m_collisionEventActorsChanges) + m_collisionEventActorsChanges[obj.LocalID] = obj; } /// /// Remove actor from the list that should receive collision events in the simulate loop. /// /// - public void RemoveCollisionEventReporting(PhysicsActor obj) - { - if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) - _collisionEventPrimRemove.Add(obj); - } - - public override float TimeDilation + internal void RemoveCollisionEventReporting(PhysicsActor obj) { - get { return m_timeDilation; } - } +// m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID); - public override bool SupportsNINJAJoints - { - get { return false; } + lock (m_collisionEventActorsChanges) + m_collisionEventActorsChanges[obj.LocalID] = null; } #region Add/Remove Entities @@ -1334,613 +1963,1266 @@ namespace OpenSim.Region.Physics.OdePlugin pos.X = position.X; pos.Y = position.Y; pos.Z = position.Z; - OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + + OdeCharacter newAv + = new OdeCharacter( + avName, this, pos, size, avPIDD, avPIDP, + avCapRadius, avStandupTensor, avDensity, + avMovementDivisorWalk, avMovementDivisorRun); + newAv.Flying = isFlying; newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; return newAv; } - public void AddCharacter(OdeCharacter chr) + public override void RemoveAvatar(PhysicsActor actor) { - lock (_characters) - { - if (!_characters.Contains(chr)) - { - _characters.Add(chr); - if (chr.bad) - m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); - } - } - } +// m_log.DebugFormat( +// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}", +// actor.Name, actor.LocalID, Name); - public void RemoveCharacter(OdeCharacter chr) - { - lock (_characters) - { - if (_characters.Contains(chr)) - { - _characters.Remove(chr); - } - } + ((OdeCharacter) actor).Destroy(); } - public void BadCharacter(OdeCharacter chr) + internal void AddCharacter(OdeCharacter chr) { - lock (_badCharacter) + if (!_characters.Contains(chr)) { - if (!_badCharacter.Contains(chr)) - _badCharacter.Add(chr); - } - } - - public override void RemoveAvatar(PhysicsActor actor) - { - //m_log.Debug("[PHYSICS]:ODELOCK"); - ((OdeCharacter) actor).Destroy(); - } + _characters.Add(chr); +// m_log.DebugFormat( +// "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}. Count now {3}", +// chr.Name, chr.LocalID, Name, _characters.Count); - public void addActivePrim(OdePrim activatePrim) - { - // adds active prim.. - lock (_activeprims) + if (chr.bad) + m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid); + } + else { - if (!_activeprims.Contains(activatePrim)) - _activeprims.Add(activatePrim); + m_log.ErrorFormat( + "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!", + chr.Name, chr.LocalID); } } - public void addActiveGroups(OdePrim activatePrim) + internal void RemoveCharacter(OdeCharacter chr) { - lock (_activegroups) + if (_characters.Contains(chr)) + { + _characters.Remove(chr); + +// m_log.DebugFormat( +// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}. Count now {3}", +// chr.Name, chr.LocalID, Name, _characters.Count); + } + else { - if (!_activegroups.Contains(activatePrim)) - _activegroups.Add(activatePrim); + m_log.ErrorFormat( + "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!", + chr.Name, chr.LocalID); } } private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) + PrimitiveBaseShape pbs, bool isphysical, uint localID) { + Vector3 pos = position; + Vector3 siz = size; + Quaternion rot = rotation; + OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); + lock (_prims) _prims.Add(newPrim); } + newPrim.LocalID = localID; return newPrim; } - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) + /// + /// Make this prim subject to physics. + /// + /// + internal void ActivatePrim(OdePrim prim) { - return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); + // adds active prim.. (ones that should be iterated over in collisions_optimized + if (!_activeprims.Contains(prim)) + _activeprims.Add(prim); + //else + // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); } - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { - return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); +// m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name); + + return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid); } - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) + public override float TimeDilation { - - return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); + get { return m_timeDilation; } } - public void remActivePrim(OdePrim deactivatePrim) + public override bool SupportsNINJAJoints { - lock (_activeprims) - { - _activeprims.Remove(deactivatePrim); - } + get { return m_NINJA_physics_joints_enabled; } } - public void remActiveGroup(OdePrim deactivatePrim) + + // internal utility function: must be called within a lock (OdeLock) + private void InternalAddActiveJoint(PhysicsJoint joint) { - lock (_activegroups) - { - _activegroups.Remove(deactivatePrim); - } + activeJoints.Add(joint); + SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint); } - public override void RemovePrim(PhysicsActor prim) + // internal utility function: must be called within a lock (OdeLock) + private void InternalAddPendingJoint(OdePhysicsJoint joint) { - // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be - // removed in the next physics simulate pass. - if (prim is OdePrim) - { -// lock (OdeLock) - { - - OdePrim p = (OdePrim)prim; - p.setPrimForRemoval(); - } - } + pendingJoints.Add(joint); + SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint); } - /// - /// This is called from within simulate but outside the locked portion - /// We need to do our own locking here - /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in - /// Simulate() -- justincc). - /// - /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. - /// - /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory - /// that the space was using. - /// - /// - public void RemovePrimThreadLocked(OdePrim prim) - { - //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); - lock (prim) - { -// RemoveCollisionEventReporting(prim); - lock (_prims) - _prims.Remove(prim); - } + // internal utility function: must be called within a lock (OdeLock) + private void InternalRemovePendingJoint(PhysicsJoint joint) + { + pendingJoints.Remove(joint); + SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene); } - public bool havePrim(OdePrim prm) + // internal utility function: must be called within a lock (OdeLock) + private void InternalRemoveActiveJoint(PhysicsJoint joint) { - lock (_prims) - return _prims.Contains(prm); + activeJoints.Remove(joint); + SOPName_to_activeJoint.Remove(joint.ObjectNameInScene); } - public bool haveActor(PhysicsActor actor) + public override void DumpJointInfo() { - if (actor is OdePrim) + string hdr = "[NINJA] JOINTINFO: "; + foreach (PhysicsJoint j in pendingJoints) { - lock (_prims) - return _prims.Contains((OdePrim)actor); + m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); } - else if (actor is OdeCharacter) + m_log.Debug(hdr + pendingJoints.Count + " total pending joints"); + foreach (string jointName in SOPName_to_pendingJoint.Keys) { - lock (_characters) - return _characters.Contains((OdeCharacter)actor); + m_log.Debug(hdr + " pending joints dict contains Name: " + jointName); } - return false; - } - - #endregion + m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries"); + foreach (PhysicsJoint j in activeJoints) + { + m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); + } + m_log.Debug(hdr + activeJoints.Count + " total active joints"); + foreach (string jointName in SOPName_to_activeJoint.Keys) + { + m_log.Debug(hdr + " active joints dict contains Name: " + jointName); + } + m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries"); - #region Space Separation Calculation + m_log.Debug(hdr + " Per-body joint connectivity information follows."); + m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints."); + foreach (string actorName in joints_connecting_actor.Keys) + { + m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it"); + foreach (PhysicsJoint j in joints_connecting_actor[actorName]) + { + m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); + } + m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor"); + } + } - /// - /// Called when a static prim moves or becomes static - /// Places the prim in a space one the static sub-spaces grid - /// - /// the pointer to the geom that moved - /// the position that the geom moved to - /// a pointer to the space it was in before it was moved. - /// a pointer to the new space it's in - public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) + public override void RequestJointDeletion(string ObjectNameInScene) { - // moves a prim into another static sub-space or from another space into a static sub-space - - // Called ODEPrim so - // it's already in locked space. - - if (geom == IntPtr.Zero) // shouldn't happen - return IntPtr.Zero; - - // get the static sub-space for current position - IntPtr newspace = calculateSpaceForGeom(pos); + lock (externalJointRequestsLock) + { + if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously + { + requestedJointsToBeDeleted.Add(ObjectNameInScene); + } + } + } - if (newspace == currentspace) // if we are there all done - return newspace; + private void DeleteRequestedJoints() + { + List myRequestedJointsToBeDeleted; + lock (externalJointRequestsLock) + { + // make a local copy of the shared list for processing (threading issues) + myRequestedJointsToBeDeleted = new List(requestedJointsToBeDeleted); + } - // else remove it from its current space - if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) + foreach (string jointName in myRequestedJointsToBeDeleted) { - if (d.GeomIsSpace(currentspace)) + lock (OdeLock) { - waitForSpaceUnlock(currentspace); - d.SpaceRemove(currentspace, geom); + //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName); + if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName)) + { + OdePhysicsJoint joint = null; + if (SOPName_to_activeJoint.ContainsKey(jointName)) + { + joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint; + InternalRemoveActiveJoint(joint); + } + else if (SOPName_to_pendingJoint.ContainsKey(jointName)) + { + joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint; + InternalRemovePendingJoint(joint); + } + + if (joint != null) + { + //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames); + for (int iBodyName = 0; iBodyName < 2; iBodyName++) + { + string bodyName = joint.BodyNames[iBodyName]; + if (bodyName != "NULL") + { + joints_connecting_actor[bodyName].Remove(joint); + if (joints_connecting_actor[bodyName].Count == 0) + { + joints_connecting_actor.Remove(bodyName); + } + } + } - if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) + DoJointDeactivated(joint); + if (joint.jointID != IntPtr.Zero) + { + d.JointDestroy(joint.jointID); + joint.jointID = IntPtr.Zero; + //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName); + } + else + { + //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName); + } + } + else + { + // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName); + } + } + else { - d.SpaceDestroy(currentspace); + // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName); } } - else + } + + // remove processed joints from the shared list + lock (externalJointRequestsLock) + { + foreach (string jointName in myRequestedJointsToBeDeleted) { - m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + - " Geom:" + geom); + requestedJointsToBeDeleted.Remove(jointName); } } - else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space + } + + // for pending joints we don't know if their associated bodies exist yet or not. + // the joint is actually created during processing of the taints + private void CreateRequestedJoints() + { + List myRequestedJointsToBeCreated; + lock (externalJointRequestsLock) + { + // make a local copy of the shared list for processing (threading issues) + myRequestedJointsToBeCreated = new List(requestedJointsToBeCreated); + } + + foreach (PhysicsJoint joint in myRequestedJointsToBeCreated) { - currentspace = d.GeomGetSpace(geom); - if (currentspace != IntPtr.Zero) + lock (OdeLock) { - if (d.GeomIsSpace(currentspace)) + if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null) { - waitForSpaceUnlock(currentspace); - d.SpaceRemove(currentspace, geom); + DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation); + continue; + } + if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null) + { + DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation); + continue; + } - if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) + InternalAddPendingJoint(joint as OdePhysicsJoint); + + if (joint.BodyNames.Count >= 2) + { + for (int iBodyName = 0; iBodyName < 2; iBodyName++) { - d.SpaceDestroy(currentspace); + string bodyName = joint.BodyNames[iBodyName]; + if (bodyName != "NULL") + { + if (!joints_connecting_actor.ContainsKey(bodyName)) + { + joints_connecting_actor.Add(bodyName, new List()); + } + joints_connecting_actor[bodyName].Add(joint); + } } - } } } - // put the geom in the newspace - waitForSpaceUnlock(newspace); - d.SpaceAdd(newspace, geom); - - // let caller know this newspace - return newspace; + // remove processed joints from shared list + lock (externalJointRequestsLock) + { + foreach (PhysicsJoint joint in myRequestedJointsToBeCreated) + { + requestedJointsToBeCreated.Remove(joint); + } + } } /// - /// Calculates the space the prim should be in by its position + /// Add a request for joint creation. /// - /// - /// a pointer to the space. This could be a new space or reused space. - public IntPtr calculateSpaceForGeom(Vector3 pos) + /// + /// this joint will just be added to a waiting list that is NOT processed during the main + /// Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + public override PhysicsJoint RequestJointCreation( + string objectNameInScene, PhysicsJointType jointType, Vector3 position, + Quaternion rotation, string parms, List bodyNames, string trackedBodyName, Quaternion localRotation) { - int x, y; + OdePhysicsJoint joint = new OdePhysicsJoint(); + joint.ObjectNameInScene = objectNameInScene; + joint.Type = jointType; + joint.Position = position; + joint.Rotation = rotation; + joint.RawParams = parms; + joint.BodyNames = new List(bodyNames); + joint.TrackedBodyName = trackedBodyName; + joint.LocalRotation = localRotation; + joint.jointID = IntPtr.Zero; + joint.ErrorMessageCount = 0; + + lock (externalJointRequestsLock) + { + if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice + { + requestedJointsToBeCreated.Add(joint); + } + } - if (pos.X < 0) - return staticPrimspaceOffRegion[0]; + return joint; + } - if (pos.Y < 0) - return staticPrimspaceOffRegion[2]; + private void RemoveAllJointsConnectedToActor(PhysicsActor actor) + { + //m_log.Debug("RemoveAllJointsConnectedToActor: start"); + if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null) + { + List jointsToRemove = new List(); + //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism) + foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName]) + { + jointsToRemove.Add(j); + } + foreach (PhysicsJoint j in jointsToRemove) + { + //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene); + RequestJointDeletion(j.ObjectNameInScene); + //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene); + j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing) + } + } + } - x = (int)(pos.X * spacesPerMeter); - if (x > spaceGridMaxX) - return staticPrimspaceOffRegion[1]; - - y = (int)(pos.Y * spacesPerMeter); - if (y > spaceGridMaxY) - return staticPrimspaceOffRegion[3]; + public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor) + { + //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start"); + lock (OdeLock) + { + //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock"); + RemoveAllJointsConnectedToActor(actor); + } + } + + // normally called from within OnJointMoved, which is called from within a lock (OdeLock) + public override Vector3 GetJointAnchor(PhysicsJoint joint) + { + Debug.Assert(joint.IsInPhysicsEngine); + d.Vector3 pos = new d.Vector3(); - return staticPrimspace[x, y]; + if (!(joint is OdePhysicsJoint)) + { + DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); + } + else + { + OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint; + switch (odeJoint.Type) + { + case PhysicsJointType.Ball: + d.JointGetBallAnchor(odeJoint.jointID, out pos); + break; + case PhysicsJointType.Hinge: + d.JointGetHingeAnchor(odeJoint.jointID, out pos); + break; + } + } + return new Vector3(pos.X, pos.Y, pos.Z); } - - #endregion + /// + /// Get joint axis. + /// + /// + /// normally called from within OnJointMoved, which is called from within a lock (OdeLock) + /// WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function + /// appears to be unreliable. Fortunately we can compute the joint axis ourselves by + /// keeping track of the joint's original orientation relative to one of the involved bodies. + /// + /// + /// + public override Vector3 GetJointAxis(PhysicsJoint joint) + { + Debug.Assert(joint.IsInPhysicsEngine); + d.Vector3 axis = new d.Vector3(); + + if (!(joint is OdePhysicsJoint)) + { + DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); + } + else + { + OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint; + switch (odeJoint.Type) + { + case PhysicsJointType.Ball: + DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene); + break; + case PhysicsJointType.Hinge: + d.JointGetHingeAxis(odeJoint.jointID, out axis); + break; + } + } + return new Vector3(axis.X, axis.Y, axis.Z); + } /// - /// Called to queue a change to a actor - /// to use in place of old taint mechanism so changes do have a time sequence + /// Stop this prim being subject to physics /// + /// + internal void DeactivatePrim(OdePrim prim) + { + _activeprims.Remove(prim); + } - public void AddChange(PhysicsActor actor, changes what, Object arg) + public override void RemovePrim(PhysicsActor prim) { - ODEchangeitem item = new ODEchangeitem(); - item.actor = actor; - item.what = what; - item.arg = arg; - ChangesQueue.Enqueue(item); + // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be + // removed in the next physics simulate pass. + if (prim is OdePrim) + { + lock (OdeLock) + { + OdePrim p = (OdePrim) prim; + + p.setPrimForRemoval(); + AddPhysicsActorTaint(prim); + } + } } /// - /// Called after our prim properties are set Scale, position etc. - /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex - /// This assures us that we have no race conditions + /// This is called from within simulate but outside the locked portion + /// We need to do our own locking here + /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in + /// Simulate() -- justincc). + /// + /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. + /// + /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory + /// that the space was using. /// /// - public override void AddPhysicsActorTaint(PhysicsActor prim) + internal void RemovePrimThreadLocked(OdePrim prim) { +// m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID); + + lock (prim) + { + RemoveCollisionEventReporting(prim); + + if (prim.prim_geom != IntPtr.Zero) + { + prim.ResetTaints(); + + if (prim.IsPhysical) + { + prim.disableBody(); + if (prim.childPrim) + { + prim.childPrim = false; + prim.Body = IntPtr.Zero; + prim.m_disabled = true; + prim.IsPhysical = false; + } + + + } + // we don't want to remove the main space + + // If the geometry is in the targetspace, remove it from the target space + //m_log.Warn(prim.m_targetSpace); + + //if (prim.m_targetSpace != IntPtr.Zero) + //{ + //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom)) + //{ + + //if (d.GeomIsSpace(prim.m_targetSpace)) + //{ + //waitForSpaceUnlock(prim.m_targetSpace); + //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom); + prim.m_targetSpace = IntPtr.Zero; + //} + //else + //{ + // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + + //((OdePrim)prim).m_targetSpace.ToString()); + //} + + //} + //} + //m_log.Warn(prim.prim_geom); + + if (!prim.RemoveGeom()) + m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene"); + + lock (_prims) + _prims.Remove(prim); + + //If there are no more geometries in the sub-space, we don't need it in the main space anymore + //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0) + //{ + //if (prim.m_targetSpace != null) + //{ + //if (d.GeomIsSpace(prim.m_targetSpace)) + //{ + //waitForSpaceUnlock(prim.m_targetSpace); + //d.SpaceRemove(space, prim.m_targetSpace); + // free up memory used by the space. + //d.SpaceDestroy(prim.m_targetSpace); + //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position); + //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]); + //} + //else + //{ + //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + + //((OdePrim) prim).m_targetSpace.ToString()); + //} + //} + //} + + if (SupportsNINJAJoints) + RemoveAllJointsConnectedToActorThreadLocked(prim); + } + } } - // does all pending changes generated during region load process - public override void PrepareSimulation() + #endregion + + #region Space Separation Calculation + + /// + /// Takes a space pointer and zeros out the array we're using to hold the spaces + /// + /// + private void resetSpaceArrayItemToZero(IntPtr pSpace) { - lock (OdeLock) + for (int x = 0; x < staticPrimspace.GetLength(0); x++) { - if (world == IntPtr.Zero) + for (int y = 0; y < staticPrimspace.GetLength(1); y++) { - ChangesQueue.Clear(); - return; + if (staticPrimspace[x, y] == pSpace) + staticPrimspace[x, y] = IntPtr.Zero; } + } + } - ODEchangeitem item; +// private void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY) +// { +// staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero; +// } - int donechanges = 0; - if (ChangesQueue.Count > 0) + /// + /// Called when a static prim moves. Allocates a space for the prim based on its position + /// + /// the pointer to the geom that moved + /// the position that the geom moved to + /// a pointer to the space it was in before it was moved. + /// a pointer to the new space it's in + internal IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace) + { + // Called from setting the Position and Size of an ODEPrim so + // it's already in locked space. + + // we don't want to remove the main space + // we don't need to test physical here because this function should + // never be called if the prim is physical(active) + + // All physical prim end up in the root space + //Thread.Sleep(20); + if (currentspace != space) + { + //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString()); + //if (currentspace == IntPtr.Zero) + //{ + //int adfadf = 0; + //} + if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { +// waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + else + { + m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + currentspace + + " Geom:" + geom); + } + } + else { - m_log.InfoFormat("[ODE] start processing pending actor operations"); - int tstart = Util.EnvironmentTickCount(); + IntPtr sGeomIsIn = d.GeomGetSpace(geom); + if (sGeomIsIn != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { +// waitForSpaceUnlock(sGeomIsIn); + d.SpaceRemove(sGeomIsIn, geom); + } + else + { + m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + + sGeomIsIn + " Geom:" + geom); + } + } + } - while (ChangesQueue.Dequeue(out item)) + //If there are no more geometries in the sub-space, we don't need it in the main space anymore + if (d.SpaceGetNumGeoms(currentspace) == 0) + { + if (currentspace != IntPtr.Zero) { - if (item.actor != null) + if (d.GeomIsSpace(currentspace)) { - try +// waitForSpaceUnlock(currentspace); +// waitForSpaceUnlock(space); + d.SpaceRemove(space, currentspace); + // free up memory used by the space. + + //d.SpaceDestroy(currentspace); + resetSpaceArrayItemToZero(currentspace); + } + else + { + m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + + currentspace + " Geom:" + geom); + } + } + } + } + else + { + // this is a physical object that got disabled. ;.; + if (currentspace != IntPtr.Zero && geom != IntPtr.Zero) + { + if (d.SpaceQuery(currentspace, geom)) + { + if (d.GeomIsSpace(currentspace)) + { +// waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + else + { + m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + + currentspace + " Geom:" + geom); + } + } + else + { + IntPtr sGeomIsIn = d.GeomGetSpace(geom); + if (sGeomIsIn != IntPtr.Zero) + { + if (d.GeomIsSpace(sGeomIsIn)) { - if (item.actor is OdeCharacter) - ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); - else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) - RemovePrimThreadLocked((OdePrim)item.actor); +// waitForSpaceUnlock(sGeomIsIn); + d.SpaceRemove(sGeomIsIn, geom); } - catch + else { - m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", - item.actor.Name, item.what.ToString()); + m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + + sGeomIsIn + " Geom:" + geom); } } - donechanges++; } - int time = Util.EnvironmentTickCountSubtract(tstart); - m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); + } + } + + // The routines in the Position and Size sections do the 'inserting' into the space, + // so all we have to do is make sure that the space that we're putting the prim into + // is in the 'main' space. + int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos); + IntPtr newspace = calculateSpaceForGeom(pos); + + if (newspace == IntPtr.Zero) + { + newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); + d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh); + } + + return newspace; + } + + /// + /// Creates a new space at X Y + /// + /// + /// + /// A pointer to the created space + internal IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY) + { + // creating a new space for prim and inserting it into main space. + staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero); + d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space); +// waitForSpaceUnlock(space); + d.SpaceSetSublevel(space, 1); + d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]); + + return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]; + } + + /// + /// Calculates the space the prim should be in by its position + /// + /// + /// a pointer to the space. This could be a new space or reused space. + internal IntPtr calculateSpaceForGeom(Vector3 pos) + { + int[] xyspace = calculateSpaceArrayItemFromPos(pos); + //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString()); + return staticPrimspace[xyspace[0], xyspace[1]]; + } + + /// + /// Holds the space allocation logic + /// + /// + /// an array item based on the position + internal int[] calculateSpaceArrayItemFromPos(Vector3 pos) + { + int[] returnint = new int[2]; + + returnint[0] = (int) (pos.X/metersInSpace); + + if (returnint[0] > ((int) (259f/metersInSpace))) + returnint[0] = ((int) (259f/metersInSpace)); + if (returnint[0] < 0) + returnint[0] = 0; + + returnint[1] = (int) (pos.Y/metersInSpace); + if (returnint[1] > ((int) (259f/metersInSpace))) + returnint[1] = ((int) (259f/metersInSpace)); + if (returnint[1] < 0) + returnint[1] = 0; + + return returnint; + } + + #endregion + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// + internal bool needsMeshing(PrimitiveBaseShape pbs) + { + // most of this is redundant now as the mesher will return null if it cant mesh a prim + // but we still need to check for sculptie meshing being enabled so this is the most + // convenient place to do it for now... + + // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f) + // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString()); + int iPropertiesNotSupportedDefault = 0; + + if (pbs.SculptEntry && !meshSculptedPrim) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } + + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim + if (!forceSimplePrimMeshing && !pbs.SculptEntry) + { + if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) + || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 + && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) + { + + if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 + && pbs.ProfileHollow == 0 + && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 + && pbs.PathBegin == 0 && pbs.PathEnd == 0 + && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 + && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 + && pbs.PathShearX == 0 && pbs.PathShearY == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } + } + } + + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + + if (pbs.SculptEntry && meshSculptedPrim) + iPropertiesNotSupportedDefault++; + + if (iPropertiesNotSupportedDefault == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } +#if SPAM + m_log.Debug("Mesh"); +#endif + return true; + } + + /// + /// Called after our prim properties are set Scale, position etc. + /// + /// + /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex + /// This assures us that we have no race conditions + /// + /// + public override void AddPhysicsActorTaint(PhysicsActor actor) + { + if (actor is OdePrim) + { + OdePrim taintedprim = ((OdePrim)actor); + lock (_taintedPrims) + _taintedPrims.Add(taintedprim); + } + else if (actor is OdeCharacter) + { + OdeCharacter taintedchar = ((OdeCharacter)actor); + lock (_taintedActors) + { + _taintedActors.Add(taintedchar); + if (taintedchar.bad) + m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid); } } } /// /// This is our main simulate loop + /// + /// /// It's thread locked by a Mutex in the scene. /// It holds Collisions, it instructs ODE to step through the physical reactions /// It moves the objects around in memory /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) - /// + /// /// - /// + /// The number of frames simulated over that period. public override float Simulate(float timeStep) { + if (!_worldInitialized) return 11f; - DateTime now = DateTime.UtcNow; - TimeSpan timedif = now - m_lastframe; - m_lastframe = now; - timeStep = (float)timedif.TotalSeconds; - - // acumulate time so we can reduce error - step_time += timeStep; + int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0; + int tempTick = 0, tempTick2 = 0; - if (step_time < HalfOdeStep) - return 0; - - if (framecount < 0) + if (framecount >= int.MaxValue) framecount = 0; framecount++; - int curphysiteractions; + float fps = 0; - // if in trouble reduce step resolution - if (step_time >= m_SkipFramesAtms) - curphysiteractions = m_physicsiterations / 2; - else - curphysiteractions = m_physicsiterations; + float timeLeft = timeStep; + + //m_log.Info(timeStep.ToString()); +// step_time += timeSte +// +// // If We're loaded down by something else, +// // or debugging with the Visual Studio project on pause +// // skip a few frames to catch up gracefully. +// // without shooting the physicsactors all over the place +// +// if (step_time >= m_SkipFramesAtms) +// { +// // Instead of trying to catch up, it'll do 5 physics frames only +// step_time = ODE_STEPSIZE; +// m_physicsiterations = 5; +// } +// else +// { +// m_physicsiterations = 10; +// } + + // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential + // deadlock if the collision event tries to lock something else later on which is already locked by a + // caller that is adding or removing the collision event. + lock (m_collisionEventActorsChanges) + { + foreach (KeyValuePair kvp in m_collisionEventActorsChanges) + { + if (kvp.Value == null) + m_collisionEventActors.Remove(kvp.Key); + else + m_collisionEventActors[kvp.Key] = kvp.Value; + } - int nodeframes = 0; + m_collisionEventActorsChanges.Clear(); + } -// checkThread(); + if (SupportsNINJAJoints) + { + DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks + CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks + } - lock (SimulationLock) - lock(OdeLock) + lock (OdeLock) { - if (world == IntPtr.Zero) + // Process 10 frames if the sim is running normal.. + // process 5 frames if the sim is running slow + //try + //{ + //d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + //} + //catch (StackOverflowException) + //{ + // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); + // ode.drelease(world); + //base.TriggerPhysicsBasedRestart(); + //} + + // Figure out the Frames Per Second we're going at. + //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size + + fps = (timeStep / ODE_STEPSIZE) * 1000; + // HACK: Using a time dilation of 1.0 to debug rubberbanding issues + //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f); + + while (timeLeft > 0.0f) { - ChangesQueue.Clear(); - return 0; - } + try + { + if (CollectStats) + tempTick = Util.EnvironmentTickCount(); + + lock (_taintedActors) + { + foreach (OdeCharacter character in _taintedActors) + character.ProcessTaints(); - ODEchangeitem item; + _taintedActors.Clear(); + } - if (ChangesQueue.Count > 0) - { - int ttmpstart = Util.EnvironmentTickCount(); - int ttmp; + if (CollectStats) + { + tempTick2 = Util.EnvironmentTickCount(); + m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); + tempTick = tempTick2; + } - while (ChangesQueue.Dequeue(out item)) - { - if (item.actor != null) + lock (_taintedPrims) { - try - { - if (item.actor is OdeCharacter) - ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); - else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) - RemovePrimThreadLocked((OdePrim)item.actor); - } - catch + foreach (OdePrim prim in _taintedPrims) { - m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", - item.actor.Name, item.what.ToString()); + if (prim.m_taintremove) + { +// Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name); + RemovePrimThreadLocked(prim); + } + else + { +// Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name); + prim.ProcessTaints(); + } + + prim.m_collisionscore = 0; + + // This loop can block up the Heartbeat for a very long time on large regions. + // We need to let the Watchdog know that the Heartbeat is not dead + // NOTE: This is currently commented out, but if things like OAR loading are + // timing the heartbeat out we will need to uncomment it + //Watchdog.UpdateThread(); } - } - ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); - if (ttmp > 20) - break; - } - } - - d.WorldSetQuickStepNumIterations(world, curphysiteractions); - while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever - { - try - { - // clear pointer/counter to contacts to pass into joints - m_global_contactcount = 0; + if (SupportsNINJAJoints) + SimulatePendingNINJAJoints(); + _taintedPrims.Clear(); + } + + if (CollectStats) + { + tempTick2 = Util.EnvironmentTickCount(); + m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); + tempTick = tempTick2; + } // Move characters - lock (_characters) + foreach (OdeCharacter actor in _characters) + actor.Move(defects); + + if (defects.Count != 0) { - List defects = new List(); - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - actor.Move(ODE_STEPSIZE, defects); - } - if (defects.Count != 0) + foreach (OdeCharacter actor in defects) { - foreach (OdeCharacter defect in defects) - { - RemoveCharacter(defect); - } - defects.Clear(); + m_log.ErrorFormat( + "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving", + actor.Name, actor.LocalID, Name); + + RemoveCharacter(actor); + actor.DestroyOdeStructures(); } + + defects.Clear(); + } + + if (CollectStats) + { + tempTick2 = Util.EnvironmentTickCount(); + m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); + tempTick = tempTick2; } // Move other active objects - lock (_activegroups) + foreach (OdePrim prim in _activeprims) { - foreach (OdePrim aprim in _activegroups) - { - aprim.Move(); - } + prim.m_collisionscore = 0; + prim.Move(timeStep); + } + + if (CollectStats) + { + tempTick2 = Util.EnvironmentTickCount(); + m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); + tempTick = tempTick2; } //if ((framecount % m_randomizeWater) == 0) - // randomizeWater(waterlevel); + // randomizeWater(waterlevel); + //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests(); m_rayCastManager.ProcessQueuedRequests(); + if (CollectStats) + { + tempTick2 = Util.EnvironmentTickCount(); + m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); + tempTick = tempTick2; + } + collision_optimized(); - foreach (PhysicsActor obj in _collisionEventPrim) + if (CollectStats) + { + tempTick2 = Util.EnvironmentTickCount(); + m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); + tempTick = tempTick2; + } + + foreach (PhysicsActor obj in m_collisionEventActors.Values) { - if (obj == null) - continue; +// m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID); switch ((ActorTypes)obj.PhysicsActorType) { case ActorTypes.Agent: OdeCharacter cobj = (OdeCharacter)obj; - cobj.AddCollisionFrameTime((int)(odetimestepMS)); + cobj.AddCollisionFrameTime(100); cobj.SendCollisions(); break; case ActorTypes.Prim: OdePrim pobj = (OdePrim)obj; - if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) - if (!pobj.m_outbounds) - { - pobj.AddCollisionFrameTime((int)(odetimestepMS)); - pobj.SendCollisions(); - } + pobj.SendCollisions(); break; } } - foreach (PhysicsActor obj in _collisionEventPrimRemove) - _collisionEventPrim.Remove(obj); - - _collisionEventPrimRemove.Clear(); +// if (m_global_contactcount > 0) +// m_log.DebugFormat( +// "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount); - // do a ode simulation step - d.WorldQuickStep(world, ODE_STEPSIZE); - d.JointGroupEmpty(contactgroup); + m_global_contactcount = 0; - // update managed ideia of physical data and do updates to core - /* - lock (_characters) + if (CollectStats) { - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - { - if (actor.bad) - m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); - - actor.UpdatePositionAndVelocity(); - } - } + tempTick2 = Util.EnvironmentTickCount(); + m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); + tempTick = tempTick2; } - */ - lock (_activegroups) - { - { - foreach (OdePrim actor in _activegroups) - { - if (actor.IsPhysical) - { - actor.UpdatePositionAndVelocity(); - } - } - } - } + d.WorldQuickStep(world, ODE_STEPSIZE); + + if (CollectStats) + m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); + + d.JointGroupEmpty(contactgroup); } catch (Exception e) { - m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); -// ode.dunlock(world); + m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e); } + timeLeft -= ODE_STEPSIZE; + } + + if (CollectStats) + tempTick = Util.EnvironmentTickCount(); + + foreach (OdeCharacter actor in _characters) + { + if (actor.bad) + m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); - step_time -= ODE_STEPSIZE; - nodeframes++; + actor.UpdatePositionAndVelocity(defects); } - lock (_badCharacter) + if (defects.Count != 0) { - if (_badCharacter.Count > 0) + foreach (OdeCharacter actor in defects) { - foreach (OdeCharacter chr in _badCharacter) - { - RemoveCharacter(chr); - } + m_log.ErrorFormat( + "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity", + actor.Name, actor.LocalID, Name); - _badCharacter.Clear(); + RemoveCharacter(actor); + actor.DestroyOdeStructures(); } - } - timedif = now - m_lastMeshExpire; + defects.Clear(); + } - if (timedif.Seconds > 10) + if (CollectStats) { - mesher.ExpireReleaseMeshs(); - m_lastMeshExpire = now; + tempTick2 = Util.EnvironmentTickCount(); + m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); + tempTick = tempTick2; } -// information block running in debug only -/* - int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); - int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); - int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); - - int nactivegeoms = 0; - int nactivespaces = 0; + //if (timeStep < 0.2f) - int nstaticgeoms = 0; - int nstaticspaces = 0; - IntPtr sp; - - for (int i = 0; i < ntopactivegeoms; i++) + foreach (OdePrim prim in _activeprims) { - sp = d.SpaceGetGeom(ActiveSpace, i); - if (d.GeomIsSpace(sp)) + if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag)) { - nactivespaces++; - nactivegeoms += d.SpaceGetNumGeoms(sp); - } - else - nactivegeoms++; - } + prim.UpdatePositionAndVelocity(); - for (int i = 0; i < ntopstaticgeoms; i++) - { - sp = d.SpaceGetGeom(StaticSpace, i); - if (d.GeomIsSpace(sp)) - { - nstaticspaces++; - nstaticgeoms += d.SpaceGetNumGeoms(sp); + if (SupportsNINJAJoints) + SimulateActorPendingJoints(prim); } - else - nstaticgeoms++; } - int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); + if (CollectStats) + m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); + + //DumpJointInfo(); - int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray - int nbodies = d.NTotalBodies; - int ngeoms = d.NTotalGeoms; -*/ // Finished with all sim stepping. If requested, dump world state to file for debugging. // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? @@ -1959,26 +3241,256 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); } - - // think time dilation as to do with dinamic step size that we dont' have - // even so tell something to world - if (nodeframes < 10) // we did the requested loops + + latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun); + + // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics + // has a max of 100 ms to run theoretically. + // If the main loop stalls, it calls Simulate later which makes the tick count ms larger. + // If Physics stalls, it takes longer which makes the tick count ms larger. + + if (latertickcount < 100) + { m_timeDilation = 1.0f; - else if (step_time > 0) + } + else { - m_timeDilation = timeStep / step_time; - if (m_timeDilation > 1) - m_timeDilation = 1; - if (step_time > m_SkipFramesAtms) - step_time = 0; + m_timeDilation = 100f / latertickcount; + //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f); } + + tickCountFrameRun = Util.EnvironmentTickCount(); + + if (CollectStats) + m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick); } -// return nodeframes * ODE_STEPSIZE; // return real simulated time - return 1000 * nodeframes; // return steps for now * 1000 to keep core happy + return fps; + } + + /// + /// Simulate pending NINJA joints. + /// + /// + /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else. + /// + private void SimulatePendingNINJAJoints() + { + // Create pending joints, if possible + + // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating + // a joint requires specifying the body id of both involved bodies + if (pendingJoints.Count > 0) + { + List successfullyProcessedPendingJoints = new List(); + //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints"); + foreach (PhysicsJoint joint in pendingJoints) + { + //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams); + string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries); + List jointBodies = new List(); + bool allJointBodiesAreReady = true; + foreach (string jointParam in jointParams) + { + if (jointParam == "NULL") + { + //DoJointErrorMessage(joint, "attaching NULL joint to world"); + jointBodies.Add(IntPtr.Zero); + } + else + { + //DoJointErrorMessage(joint, "looking for prim name: " + jointParam); + bool foundPrim = false; + lock (_prims) + { + foreach (OdePrim prim in _prims) // FIXME: inefficient + { + if (prim.SOPName == jointParam) + { + //DoJointErrorMessage(joint, "found for prim name: " + jointParam); + if (prim.IsPhysical && prim.Body != IntPtr.Zero) + { + jointBodies.Add(prim.Body); + foundPrim = true; + break; + } + else + { + DoJointErrorMessage(joint, "prim name " + jointParam + + " exists but is not (yet) physical; deferring joint creation. " + + "IsPhysical property is " + prim.IsPhysical + + " and body is " + prim.Body); + foundPrim = false; + break; + } + } + } + } + if (foundPrim) + { + // all is fine + } + else + { + allJointBodiesAreReady = false; + break; + } + } + } + + if (allJointBodiesAreReady) + { + //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams); + if (jointBodies[0] == jointBodies[1]) + { + DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams); + } + else + { + switch (joint.Type) + { + case PhysicsJointType.Ball: + { + IntPtr odeJoint; + //DoJointErrorMessage(joint, "ODE creating ball joint "); + odeJoint = d.JointCreateBall(world, IntPtr.Zero); + //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]); + d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]); + //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position); + d.JointSetBallAnchor(odeJoint, + joint.Position.X, + joint.Position.Y, + joint.Position.Z); + //DoJointErrorMessage(joint, "ODE joint setting OK"); + //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: "); + //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment")); + //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: "); + //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment")); + + if (joint is OdePhysicsJoint) + { + ((OdePhysicsJoint)joint).jointID = odeJoint; + } + else + { + DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!"); + } + } + break; + case PhysicsJointType.Hinge: + { + IntPtr odeJoint; + //DoJointErrorMessage(joint, "ODE creating hinge joint "); + odeJoint = d.JointCreateHinge(world, IntPtr.Zero); + //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]); + d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]); + //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position); + d.JointSetHingeAnchor(odeJoint, + joint.Position.X, + joint.Position.Y, + joint.Position.Z); + // We use the orientation of the x-axis of the joint's coordinate frame + // as the axis for the hinge. + + // Therefore, we must get the joint's coordinate frame based on the + // joint.Rotation field, which originates from the orientation of the + // joint's proxy object in the scene. + + // The joint's coordinate frame is defined as the transformation matrix + // that converts a vector from joint-local coordinates into world coordinates. + // World coordinates are defined as the XYZ coordinate system of the sim, + // as shown in the top status-bar of the viewer. + + // Once we have the joint's coordinate frame, we extract its X axis (AtAxis) + // and use that as the hinge axis. + + //joint.Rotation.Normalize(); + Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation); + + // Now extract the X axis of the joint's coordinate frame. + + // Do not try to use proxyFrame.AtAxis or you will become mired in the + // tar pit of transposed, inverted, and generally messed-up orientations. + // (In other words, Matrix4.AtAxis() is borked.) + // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness + + // Instead, compute the X axis of the coordinate frame by transforming + // the (1,0,0) vector. At least that works. + + //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame); + Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame); + //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis); + //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis); + d.JointSetHingeAxis(odeJoint, + jointAxis.X, + jointAxis.Y, + jointAxis.Z); + //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f); + if (joint is OdePhysicsJoint) + { + ((OdePhysicsJoint)joint).jointID = odeJoint; + } + else + { + DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!"); + } + } + break; + } + successfullyProcessedPendingJoints.Add(joint); + } + } + else + { + DoJointErrorMessage(joint, "joint could not yet be created; still pending"); + } + } + + foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints) + { + //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams); + //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending"); + InternalRemovePendingJoint(successfullyProcessedJoint); + //DoJointErrorMessage(successfullyProcessedJoint, "adding to active"); + InternalAddActiveJoint(successfullyProcessedJoint); + //DoJointErrorMessage(successfullyProcessedJoint, "done"); + } + } } /// + /// Simulate the joint proxies of a NINJA actor. + /// + /// + /// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there. + /// + /// + private void SimulateActorPendingJoints(OdePrim actor) + { + // If an actor moved, move its joint proxy objects as well. + // There seems to be an event PhysicsActor.OnPositionUpdate that could be used + // for this purpose but it is never called! So we just do the joint + // movement code here. + + if (actor.SOPName != null && + joints_connecting_actor.ContainsKey(actor.SOPName) && + joints_connecting_actor[actor.SOPName] != null && + joints_connecting_actor[actor.SOPName].Count > 0) + { + foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName]) + { + if (affectedJoint.IsInPhysicsEngine) + { + DoJointMoved(affectedJoint); + } + else + { + DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams); + } + } + } + } + public override void GetResults() { } @@ -1986,141 +3498,275 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool IsThreaded { // for now we won't be multithreaded - get { return (false); } + get { return false; } } - public float GetTerrainHeightAtXY(float x, float y) + #region ODE Specific Terrain Fixes + private float[] ResizeTerrain512NearestNeighbour(float[] heightMap) { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y]; - - int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - - - IntPtr heightFieldGeom = IntPtr.Zero; - - // get region map - if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) - return 0f; - - if (heightFieldGeom == IntPtr.Zero) - return 0f; - - if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) - return 0f; - - // TerrainHeightField for ODE as offset 1m - x += 1f - offsetX; - y += 1f - offsetY; - - // make position fit into array - if (x < 0) - x = 0; - if (y < 0) - y = 0; - - // integer indexs - int ix; - int iy; - // interpolators offset - float dx; - float dy; - - int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples - - if (OdeUbitLib) + // Filling out the array into its multi-dimensional components + for (int y = 0; y < WorldExtents.Y; y++) { - if (x < regsize - 1) - { - ix = (int)x; - dx = x - (float)ix; - } - else // out world use external height - { - ix = regsize - 2; - dx = 0; - } - if (y < regsize - 1) - { - iy = (int)y; - dy = y - (float)iy; - } - else + for (int x = 0; x < WorldExtents.X; x++) { - iy = regsize - 2; - dy = 0; + resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x]; } } - else + // Resize using Nearest Neighbour + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512, 512]; + for (int y = 0; y < WorldExtents.Y; y++) { - // we still have square fixed size regions - // also flip x and y because of how map is done for ODE fliped axis - // so ix,iy,dx and dy are inter exchanged - if (x < regsize - 1) - { - iy = (int)x; - dy = x - (float)iy; - } - else // out world use external height + for (int x = 0; x < WorldExtents.X; x++) { - iy = regsize - 2; - dy = 0; + resultarr2[y * 2, x * 2] = resultarr[y, x]; + + if (y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x]; + } + if (x < WorldExtents.X) + { + resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x]; + } + if (x < WorldExtents.X && y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x]; + } } - if (y < regsize - 1) + } + + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) { - ix = (int)y; - dx = y - (float)ix; + if (resultarr2[y, x] <= 0) + returnarr[i] = 0.0000001f; + else + returnarr[i] = resultarr2[y, x]; + + i++; } - else + } + + return returnarr; + } + + private float[] ResizeTerrain512Interpolation(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[512,512]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < 256; y++) + { + for (int x = 0; x < 256; x++) { - ix = regsize - 2; - dx = 0; + resultarr[y, x] = heightMap[y * 256 + x]; } } - float h0; - float h1; - float h2; - - iy *= regsize; - iy += ix; // all indexes have iy + ix + // Resize using interpolation + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512,512]; + for (int y = 0; y < (int)Constants.RegionSize; y++) + { + for (int x = 0; x < (int)Constants.RegionSize; x++) + { + resultarr2[y*2, x*2] = resultarr[y, x]; - float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; - /* - if ((dx + dy) <= 1.0f) + if (y < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2); + } + } + else + { + resultarr2[(y*2) + 1, x*2] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2); + } + } + else + { + resultarr2[y*2, (x*2) + 1] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize) + { + if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize)) { - h0 = ((float)heights[iy]); // 0,0 vertice - h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 - h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); } else { - h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice - h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 - h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; } - */ - h0 = ((float)heights[iy]); // 0,0 vertice - - if ((dy > dx)) - { - iy += regsize; - h2 = (float)heights[iy]; // 0,1 vertice - h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 - h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 + } + } } - else + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) { - iy++; - h2 = (float)heights[iy]; // vertice 1,0 - h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 - h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 + for (int x = 0; x < 512; x++) + { + if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) + { + m_log.Warn("[ODE SCENE]: Non finite heightfield element detected. Setting it to 0"); + resultarr2[y, x] = 0; + } + returnarr[i] = resultarr2[y, x]; + i++; + } } - return h0 + h1 + h2; + return returnarr; } + #endregion public override void SetTerrain(float[] heightMap) { @@ -2137,75 +3783,78 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override void CombineTerrain(float[] heightMap, Vector3 pOffset) + private void SetTerrain(float[] heightMap, Vector3 pOffset) { - SetTerrain(heightMap, pOffset); - } + int startTime = Util.EnvironmentTickCount(); + m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", Name, pOffset); - public void SetTerrain(float[] heightMap, Vector3 pOffset) - { - if (OdeUbitLib) - UbitSetTerrain(heightMap, pOffset); - else - OriSetTerrain(heightMap, pOffset); - } + // this._heightmap[i] = (double)heightMap[i]; + // dbm (danx0r) -- creating a buffer zone of one extra sample all around + //_origheightmap = heightMap; + + float[] _heightmap; - public void OriSetTerrain(float[] heightMap, Vector3 pOffset) - { - // assumes 1m size grid and constante size square regions - // needs to know about sims around in future + // zero out a heightmap array float array (single dimension [flattened])) + //if ((int)Constants.RegionSize == 256) + // _heightmap = new float[514 * 514]; + //else - float[] _heightmap; + _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; + + uint heightmapWidth = Constants.RegionSize + 1; + uint heightmapHeight = Constants.RegionSize + 1; - uint heightmapWidth = Constants.RegionSize + 2; - uint heightmapHeight = Constants.RegionSize + 2; + uint heightmapWidthSamples; - uint heightmapWidthSamples = heightmapWidth + 1; - uint heightmapHeightSamples = heightmapHeight + 1; + uint heightmapHeightSamples; - _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; + //if (((int)Constants.RegionSize) == 256) + //{ + // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2; + // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2; + // heightmapWidth++; + // heightmapHeight++; + //} + //else + //{ + + heightmapWidthSamples = (uint)Constants.RegionSize + 1; + heightmapHeightSamples = (uint)Constants.RegionSize + 1; + //} const float scale = 1.0f; const float offset = 0.0f; - const float thickness = 10f; + const float thickness = 0.2f; const int wrap = 0; - uint regionsize = Constants.RegionSize; - - float hfmin = float.MaxValue; - float hfmax = float.MinValue; - float val; - uint xx; - uint yy; + int regionsize = (int) Constants.RegionSize + 2; + //Double resolution + //if (((int)Constants.RegionSize) == 256) + // heightMap = ResizeTerrain512Interpolation(heightMap); - uint maxXXYY = regionsize - 1; - // flipping map adding one margin all around so things don't fall in edges - uint xt = 0; - xx = 0; + // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256) + // regionsize = 512; - for (uint x = 0; x < heightmapWidthSamples; x++) + float hfmin = 2000; + float hfmax = -2000; + + for (int x = 0; x < heightmapWidthSamples; x++) { - if (x > 1 && xx < maxXXYY) - xx++; - yy = 0; - for (uint y = 0; y < heightmapHeightSamples; y++) + for (int y = 0; y < heightmapHeightSamples; y++) { - if (y > 1 && y < maxXXYY) - yy += regionsize; - - val = heightMap[yy + xx]; - if (val < 0.0f) - val = 0.0f; // no neg terrain as in chode - _heightmap[xt + y] = val; - - if (hfmin > val) - hfmin = val; - if (hfmax < val) - hfmax = val; + int xx = Util.Clip(x - 1, 0, regionsize - 1); + int yy = Util.Clip(y - 1, 0, regionsize - 1); + + + float val= heightMap[yy * (int)Constants.RegionSize + xx]; + _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val; + + hfmin = (val < hfmin) ? val : hfmin; + hfmax = (val > hfmax) ? val : hfmax; } - xt += heightmapHeightSamples; } + lock (OdeLock) { IntPtr GroundGeom = IntPtr.Zero; @@ -2214,177 +3863,62 @@ namespace OpenSim.Region.Physics.OdePlugin RegionTerrain.Remove(pOffset); if (GroundGeom != IntPtr.Zero) { - actor_name_map.Remove(GroundGeom); - d.GeomDestroy(GroundGeom); - if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) - { - TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); - TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + { TerrainHeightFieldHeights.Remove(GroundGeom); - } + } + d.SpaceRemove(space, GroundGeom); + d.GeomDestroy(GroundGeom); } + } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - - GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); - - d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, - (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1, + (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale, offset, thickness, wrap); - d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); - - GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); - + GroundGeom = d.CreateHeightfield(space, HeightmapData, 1); if (GroundGeom != IntPtr.Zero) { - d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundGeom, 0); + d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); - PhysicsActor pa = new NullPhysicsActor(); - pa.Name = "Terrain"; - pa.PhysicsActorType = (int)ActorTypes.Ground; - actor_name_map[GroundGeom] = pa; - -// geom_name_map[GroundGeom] = "Terrain"; - - d.Matrix3 R = new d.Matrix3(); - - Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); - Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); - - - q1 = q1 * q2; - - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); - - d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); - d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); - RegionTerrain.Add(pOffset, GroundGeom); - TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); - TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); } - } - } - - public void UbitSetTerrain(float[] heightMap, Vector3 pOffset) - { - // assumes 1m size grid and constante size square regions - // needs to know about sims around in future - - float[] _heightmap; - - uint heightmapWidth = Constants.RegionSize + 2; - uint heightmapHeight = Constants.RegionSize + 2; - - uint heightmapWidthSamples = heightmapWidth + 1; - uint heightmapHeightSamples = heightmapHeight + 1; - - _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; + geom_name_map[GroundGeom] = "Terrain"; + d.Matrix3 R = new d.Matrix3(); - uint regionsize = Constants.RegionSize; + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); - float hfmin = float.MaxValue; -// float hfmax = float.MinValue; - float val; + q1 = q1 * q2; + //q1 = q1 * q3; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); - - uint maxXXYY = regionsize - 1; - // adding one margin all around so things don't fall in edges - - uint xx; - uint yy = 0; - uint yt = 0; - - for (uint y = 0; y < heightmapHeightSamples; y++) - { - if (y > 1 && y < maxXXYY) - yy += regionsize; - xx = 0; - for (uint x = 0; x < heightmapWidthSamples; x++) - { - if (x > 1 && x < maxXXYY) - xx++; - - val = heightMap[yy + xx]; - if (val < 0.0f) - val = 0.0f; // no neg terrain as in chode - _heightmap[yt + x] = val; - - if (hfmin > val) - hfmin = val; -// if (hfmax < val) -// hfmax = val; - } - yt += heightmapWidthSamples; - } - lock (OdeLock) - { - IntPtr GroundGeom = IntPtr.Zero; - if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(GroundGeom, ref R); + d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)), (pOffset.Y + ((int)Constants.RegionSize * 0.5f)), 0); + IntPtr testGround = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out testGround)) { RegionTerrain.Remove(pOffset); - if (GroundGeom != IntPtr.Zero) - { - actor_name_map.Remove(GroundGeom); - d.GeomDestroy(GroundGeom); - - if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) - { - if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated) - TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); - TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); - TerrainHeightFieldHeights.Remove(GroundGeom); - } - } - } - IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - - const int wrap = 0; - float thickness = hfmin; - if (thickness < 0) - thickness = 1; - - GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); - - d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, - (int)heightmapWidthSamples, (int)heightmapHeightSamples, - thickness, wrap); - -// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); - GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1); - if (GroundGeom != IntPtr.Zero) - { - d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundGeom, 0); - - - PhysicsActor pa = new NullPhysicsActor(); - pa.Name = "Terrain"; - pa.PhysicsActorType = (int)ActorTypes.Ground; - actor_name_map[GroundGeom] = pa; - -// geom_name_map[GroundGeom] = "Terrain"; - - d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); - RegionTerrain.Add(pOffset, GroundGeom); - TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); - TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); } + RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); + TerrainHeightFieldHeights.Add(GroundGeom,_heightmap); } - } + m_log.DebugFormat( + "[ODE SCENE]: Setting terrain for {0} took {1}ms", Name, Util.EnvironmentTickCountSubtract(startTime)); + } public override void DeleteTerrain() { } - public float GetWaterLevel() + internal float GetWaterLevel() { return waterlevel; } @@ -2393,252 +3927,169 @@ namespace OpenSim.Region.Physics.OdePlugin { return true; } -/* - public override void UnCombine(PhysicsScene pScene) - { - IntPtr localGround = IntPtr.Zero; -// float[] localHeightfield; - bool proceed = false; - List geomDestroyList = new List(); - - lock (OdeLock) - { - if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) - { - foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) - { - if (geom == localGround) - { -// localHeightfield = TerrainHeightFieldHeights[geom]; - proceed = true; - } - else - { - geomDestroyList.Add(geom); - } - } - - if (proceed) - { - m_worldOffset = Vector3.Zero; - WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); - m_parentScene = null; - - foreach (IntPtr g in geomDestroyList) - { - // removingHeightField needs to be done or the garbage collector will - // collect the terrain data before we tell ODE to destroy it causing - // memory corruption - if (TerrainHeightFieldHeights.ContainsKey(g)) - { -// float[] removingHeightField = TerrainHeightFieldHeights[g]; - TerrainHeightFieldHeights.Remove(g); - - if (RegionTerrain.ContainsKey(g)) - { - RegionTerrain.Remove(g); - } - d.GeomDestroy(g); - //removingHeightField = new float[0]; - } - } +// public override void UnCombine(PhysicsScene pScene) +// { +// IntPtr localGround = IntPtr.Zero; +//// float[] localHeightfield; +// bool proceed = false; +// List geomDestroyList = new List(); +// +// lock (OdeLock) +// { +// if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) +// { +// foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) +// { +// if (geom == localGround) +// { +//// localHeightfield = TerrainHeightFieldHeights[geom]; +// proceed = true; +// } +// else +// { +// geomDestroyList.Add(geom); +// } +// } +// +// if (proceed) +// { +// m_worldOffset = Vector3.Zero; +// WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); +// m_parentScene = null; +// +// foreach (IntPtr g in geomDestroyList) +// { +// // removingHeightField needs to be done or the garbage collector will +// // collect the terrain data before we tell ODE to destroy it causing +// // memory corruption +// if (TerrainHeightFieldHeights.ContainsKey(g)) +// { +//// float[] removingHeightField = TerrainHeightFieldHeights[g]; +// TerrainHeightFieldHeights.Remove(g); +// +// if (RegionTerrain.ContainsKey(g)) +// { +// RegionTerrain.Remove(g); +// } +// +// d.GeomDestroy(g); +// //removingHeightField = new float[0]; +// } +// } +// +// } +// else +// { +// m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); +// } +// } +// } +// } - } - else - { - m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); - } - } - } - } -*/ public override void SetWaterLevel(float baseheight) { waterlevel = baseheight; -// randomizeWater(waterlevel); + randomizeWater(waterlevel); } -/* - public void randomizeWater(float baseheight) - { - const uint heightmapWidth = Constants.RegionSize + 2; - const uint heightmapHeight = Constants.RegionSize + 2; - const uint heightmapWidthSamples = heightmapWidth + 1; - const uint heightmapHeightSamples = heightmapHeight + 1; + private void randomizeWater(float baseheight) + { + const uint heightmapWidth = m_regionWidth + 2; + const uint heightmapHeight = m_regionHeight + 2; + const uint heightmapWidthSamples = m_regionWidth + 2; + const uint heightmapHeightSamples = m_regionHeight + 2; const float scale = 1.0f; const float offset = 0.0f; + const float thickness = 2.9f; const int wrap = 0; - float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples]; - - float maxheigh = float.MinValue; - float minheigh = float.MaxValue; - float val; - for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++) + for (int i = 0; i < (258 * 258); i++) { - - val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f); - _watermap[i] = val; - if (maxheigh < val) - maxheigh = val; - if (minheigh > val) - minheigh = val; + _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); + // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); } - float thickness = minheigh; - lock (OdeLock) { if (WaterGeom != IntPtr.Zero) { - actor_name_map.Remove(WaterGeom); - d.GeomDestroy(WaterGeom); - d.GeomHeightfieldDataDestroy(WaterHeightmapData); - WaterGeom = IntPtr.Zero; - WaterHeightmapData = IntPtr.Zero; - if(WaterMapHandler.IsAllocated) - WaterMapHandler.Free(); + d.SpaceRemove(space, WaterGeom); } - - WaterHeightmapData = d.GeomHeightfieldDataCreate(); - - WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned); - - d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight, + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, offset, thickness, wrap); - d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh); - WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1); + d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); + WaterGeom = d.CreateHeightfield(space, HeightmapData, 1); if (WaterGeom != IntPtr.Zero) { - d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water)); - d.GeomSetCollideBits(WaterGeom, 0); - - - PhysicsActor pa = new NullPhysicsActor(); - pa.Name = "Water"; - pa.PhysicsActorType = (int)ActorTypes.Water; + d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); + d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); + } - actor_name_map[WaterGeom] = pa; -// geom_name_map[WaterGeom] = "Water"; + geom_name_map[WaterGeom] = "Water"; - d.Matrix3 R = new d.Matrix3(); + d.Matrix3 R = new d.Matrix3(); - Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); - Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); - q1 = q1 * q2; - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); + q1 = q1 * q2; + //q1 = q1 * q3; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); - d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); - d.GeomSetRotation(WaterGeom, ref R); - d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0); - } + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(WaterGeom, ref R); + d.GeomSetPosition(WaterGeom, 128, 128, 0); } } -*/ + public override void Dispose() { - if (m_meshWorker != null) - m_meshWorker.Stop(); + _worldInitialized = false; + + m_rayCastManager.Dispose(); + m_rayCastManager = null; lock (OdeLock) { - m_rayCastManager.Dispose(); - m_rayCastManager = null; - lock (_prims) { - ChangesQueue.Clear(); foreach (OdePrim prm in _prims) { - prm.DoAChange(changes.Remove, null); - _collisionEventPrim.Remove(prm); - } - _prims.Clear(); - } - - OdeCharacter[] chtorem; - lock (_characters) - { - chtorem = new OdeCharacter[_characters.Count]; - _characters.CopyTo(chtorem); - } - - ChangesQueue.Clear(); - foreach (OdeCharacter ch in chtorem) - ch.DoAChange(changes.Remove, null); - - - foreach (IntPtr GroundGeom in RegionTerrain.Values) - { - if (GroundGeom != IntPtr.Zero) - d.GeomDestroy(GroundGeom); - } - - - RegionTerrain.Clear(); - - if (TerrainHeightFieldHeightsHandlers.Count > 0) - { - foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) - { - if (gch.IsAllocated) - gch.Free(); + RemovePrim(prm); } } - TerrainHeightFieldHeightsHandlers.Clear(); - TerrainHeightFieldHeights.Clear(); -/* - if (WaterGeom != IntPtr.Zero) - { - d.GeomDestroy(WaterGeom); - WaterGeom = IntPtr.Zero; - if (WaterHeightmapData != IntPtr.Zero) - d.GeomHeightfieldDataDestroy(WaterHeightmapData); - WaterHeightmapData = IntPtr.Zero; - - if (WaterMapHandler.IsAllocated) - WaterMapHandler.Free(); - } -*/ - if (ContactgeomsArray != IntPtr.Zero) - Marshal.FreeHGlobal(ContactgeomsArray); - if (GlobalContactsArray != IntPtr.Zero) - Marshal.FreeHGlobal(GlobalContactsArray); - - + //foreach (OdeCharacter act in _characters) + //{ + //RemoveAvatar(act); + //} d.WorldDestroy(world); - world = IntPtr.Zero; //d.CloseODE(); } + } public override Dictionary GetTopColliders() { - Dictionary returncolliders = new Dictionary(); - int cnt = 0; + Dictionary topColliders; + lock (_prims) { - foreach (OdePrim prm in _prims) - { - if (prm.CollisionScore > 0) - { - returncolliders.Add(prm.LocalID, prm.CollisionScore); - cnt++; - prm.CollisionScore = 0f; - if (cnt > 25) - { - break; - } - } - } + List orderedPrims = new List(_prims); + orderedPrims.OrderByDescending(p => p.CollisionScore).Take(25); + topColliders = orderedPrims.ToDictionary(p => p.LocalID, p => p.CollisionScore); + + foreach (OdePrim p in _prims) + p.CollisionScore = 0; } - return returncolliders; + + return topColliders; } public override bool SupportsRayCast() @@ -2662,7 +4113,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - // don't like this public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) { ContactResult[] ourResults = null; @@ -2679,107 +4129,182 @@ namespace OpenSim.Region.Physics.OdePlugin waitTime++; } if (ourResults == null) - return new List(); + return new List (); return new List(ourResults); } - public override bool SuportsRaycastWorldFiltered() +#if USE_DRAWSTUFF + // Keyboard callback + public void command(int cmd) { - return true; + IntPtr geom; + d.Mass mass; + d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f); + + + + Char ch = Char.ToLower((Char)cmd); + switch ((Char)ch) + { + case 'w': + try + { + Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); + + xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z; + ds.SetViewpoint(ref xyz, ref hpr); + } + catch (ArgumentException) + { hpr.X = 0; } + break; + + case 'a': + hpr.X++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + + case 's': + try + { + Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); + + xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z; + ds.SetViewpoint(ref xyz, ref hpr); + } + catch (ArgumentException) + { hpr.X = 0; } + break; + case 'd': + hpr.X--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'r': + xyz.Z++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'f': + xyz.Z--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'e': + xyz.Y++; + ds.SetViewpoint(ref xyz, ref hpr); + break; + case 'q': + xyz.Y--; + ds.SetViewpoint(ref xyz, ref hpr); + break; + } } - public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) + public void step(int pause) { - object SyncObject = new object(); - List ourresults = new List(); - - RayCallback retMethod = delegate(List results) + + ds.SetColor(1.0f, 1.0f, 0.0f); + ds.SetTexture(ds.Texture.Wood); + lock (_prims) { - lock (SyncObject) + foreach (OdePrim prm in _prims) { - ourresults = results; - Monitor.PulseAll(SyncObject); + //IntPtr body = d.GeomGetBody(prm.prim_geom); + if (prm.prim_geom != IntPtr.Zero) + { + d.Vector3 pos; + d.GeomCopyPosition(prm.prim_geom, out pos); + //d.BodyCopyPosition(body, out pos); + + d.Matrix3 R; + d.GeomCopyRotation(prm.prim_geom, out R); + //d.BodyCopyRotation(body, out R); + + + d.Vector3 sides = new d.Vector3(); + sides.X = prm.Size.X; + sides.Y = prm.Size.Y; + sides.Z = prm.Size.Z; + + ds.DrawBox(ref pos, ref R, ref sides); + } } - }; + } + ds.SetColor(1.0f, 0.0f, 0.0f); - lock (SyncObject) + foreach (OdeCharacter chr in _characters) { - m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod); - if (!Monitor.Wait(SyncObject, 500)) - return null; - else - return ourresults; + if (chr.Shell != IntPtr.Zero) + { + IntPtr body = d.GeomGetBody(chr.Shell); + + d.Vector3 pos; + d.GeomCopyPosition(chr.Shell, out pos); + //d.BodyCopyPosition(body, out pos); + + d.Matrix3 R; + d.GeomCopyRotation(chr.Shell, out R); + //d.BodyCopyRotation(body, out R); + + ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f); + d.Vector3 sides = new d.Vector3(); + sides.X = 0.5f; + sides.Y = 0.5f; + sides.Z = 0.5f; + + ds.DrawBox(ref pos, ref R, ref sides); + } } } - public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + public void start(int unused) { - if (retMethod != null && actor !=null) - { - IntPtr geom; - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - else - return; - if (geom == IntPtr.Zero) - return; - m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod); - } + ds.SetViewpoint(ref xyz, ref hpr); } +#endif - public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + public override Dictionary GetStats() { - if (retMethod != null && actor != null) + if (!CollectStats) + return null; + + Dictionary returnStats; + + lock (OdeLock) { - IntPtr geom; - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - else - return; - if (geom == IntPtr.Zero) - return; + returnStats = new Dictionary(m_stats); - m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by + // 3 from the SimStatsReporter. + returnStats[ODETotalAvatarsStatName] = _characters.Count * 3; + returnStats[ODETotalPrimsStatName] = _prims.Count * 3; + returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3; + + InitializeExtraStats(); } + + returnStats[ODEOtherCollisionFrameMsStatName] + = returnStats[ODEOtherCollisionFrameMsStatName] + - returnStats[ODENativeSpaceCollisionFrameMsStatName] + - returnStats[ODENativeGeomCollisionFrameMsStatName]; + + return returnStats; } - // don't like this - public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + private void InitializeExtraStats() { - if (actor != null) - { - IntPtr geom; - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - else - return new List(); - if (geom == IntPtr.Zero) - return new List(); - - ContactResult[] ourResults = null; - RayCallback retMethod = delegate(List results) - { - ourResults = new ContactResult[results.Count]; - results.CopyTo(ourResults, 0); - }; - int waitTime = 0; - m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); - while (ourResults == null && waitTime < 1000) - { - Thread.Sleep(1); - waitTime++; - } - if (ourResults == null) - return new List(); - return new List(ourResults); - } - return new List(); + m_stats[ODETotalFrameMsStatName] = 0; + m_stats[ODEAvatarTaintMsStatName] = 0; + m_stats[ODEPrimTaintMsStatName] = 0; + m_stats[ODEAvatarForcesFrameMsStatName] = 0; + m_stats[ODEPrimForcesFrameMsStatName] = 0; + m_stats[ODERaycastingFrameMsStatName] = 0; + m_stats[ODENativeStepFrameMsStatName] = 0; + m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0; + m_stats[ODENativeGeomCollisionFrameMsStatName] = 0; + m_stats[ODEOtherCollisionFrameMsStatName] = 0; + m_stats[ODECollisionNotificationFrameMsStatName] = 0; + m_stats[ODEAvatarContactsStatsName] = 0; + m_stats[ODEPrimContactsStatName] = 0; + m_stats[ODEAvatarUpdateFrameMsStatName] = 0; + m_stats[ODEPrimUpdateFrameMsStatName] = 0; } } -} +} \ No newline at end of file -- cgit v1.1 From a91be67a6e7ae8682b7a672c8b49e4e6d694783c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 12 Oct 2012 00:39:58 +0100 Subject: commit the right files! --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 5483 ++++++++++++---------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 5223 ++++++++------------- 2 files changed, 4866 insertions(+), 5840 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index eaf0d0a..f083d38 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -25,6 +25,11 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +/* Revision 2011/12 by Ubit Umarov + * + * + */ + /* * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: @@ -48,250 +53,251 @@ using System.Runtime.InteropServices; using System.Threading; using log4net; using OpenMetaverse; -using Ode.NET; +using OdeAPI; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; namespace OpenSim.Region.Physics.OdePlugin { - /// - /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. - /// public class OdePrim : PhysicsActor { private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); private bool m_isphysical; + private bool m_fakeisphysical; + private bool m_isphantom; + private bool m_fakeisphantom; + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } } - private int m_expectedCollisionContacts = 0; + protected bool m_building; + protected bool m_forcePosOrRotation; + private bool m_iscolliding; - /// - /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. - /// - public override bool IsPhysical - { - get { return m_isphysical; } - set - { - m_isphysical = value; - if (!m_isphysical) // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - } - } + internal bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + internal bool m_outbounds; + + private Quaternion m_lastorientation; + private Quaternion _orientation; private Vector3 _position; private Vector3 _velocity; private Vector3 _torque; private Vector3 m_lastVelocity; private Vector3 m_lastposition; - private Quaternion m_lastorientation = new Quaternion(); private Vector3 m_rotationalVelocity; private Vector3 _size; private Vector3 _acceleration; - // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); - private Quaternion _orientation; - private Vector3 m_taintposition; - private Vector3 m_taintsize; - private Vector3 m_taintVelocity; - private Vector3 m_taintTorque; - private Quaternion m_taintrot; private Vector3 m_angularlock = Vector3.One; - private Vector3 m_taintAngularLock = Vector3.One; - private IntPtr Amotor = IntPtr.Zero; + private IntPtr Amotor; - private object m_assetsLock = new object(); - private bool m_assetFailed = false; + private Vector3 m_force; + private Vector3 m_forceacc; + private Vector3 m_angularForceacc; + + private float m_invTimeStep; + private float m_timeStep; private Vector3 m_PIDTarget; private float m_PIDTau; - private float PID_D = 35f; - private float PID_G = 25f; private bool m_usePID; - // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), - // and are for non-VEHICLES only. - private float m_PIDHoverHeight; private float m_PIDHoverTau; private bool m_useHoverPID; - private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private PIDHoverType m_PIDHoverType; private float m_targetHoverHeight; private float m_groundHeight; private float m_waterHeight; private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - // private float m_tensor = 5f; - private int body_autodisable_frames = 20; - + private int body_autodisable_frames; + public int bodydisablecontrol; - private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom - | CollisionCategories.Space - | CollisionCategories.Body - | CollisionCategories.Character - ); - private bool m_taintshape; - private bool m_taintPhysics; - private bool m_collidesLand = true; - private bool m_collidesWater; // Default we're a Geometry private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + // Default colide nonphysical don't try to colide with anything + private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; + + private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Land | + CollisionCategories.VolumeDtc); + +// private bool m_collidesLand = true; + private bool m_collidesWater; +// public bool m_returnCollisions; + + private bool m_NoColide; // for now only for internal use for bad meshs + // Default, Collide with Other Geometries, spaces and Bodies - private CollisionCategories m_collisionFlags = m_default_collisionFlags; + private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; - public bool m_taintremove { get; private set; } - public bool m_taintdisable { get; private set; } - internal bool m_disabled; - public bool m_taintadd { get; private set; } - public bool m_taintselected { get; private set; } - public bool m_taintCollidesWater { get; private set; } + public bool m_disabled; - private bool m_taintforce = false; - private bool m_taintaddangularforce = false; - private Vector3 m_force; - private List m_forcelist = new List(); - private List m_angularforcelist = new List(); + private uint m_localID; + private IMesh m_mesh; + private object m_meshlock = new object(); private PrimitiveBaseShape _pbs; - private OdeScene _parent_scene; - /// - /// The physics space which contains prim geometries - /// - public IntPtr m_targetSpace = IntPtr.Zero; + private UUID? m_assetID; + private MeshState m_meshState; + + public OdeScene _parent_scene; /// - /// The prim geometry, used for collision detection. + /// The physics space which contains prim geometry /// - /// - /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or - /// mesh change) or when the physical prim is being removed from the scene. - /// - public IntPtr prim_geom { get; private set; } + public IntPtr m_targetSpace; - public IntPtr _triMeshData { get; private set; } + public IntPtr prim_geom; + public IntPtr _triMeshData; - private IntPtr _linkJointGroup = IntPtr.Zero; private PhysicsActor _parent; - private PhysicsActor m_taintparent; private List childrenPrim = new List(); - private bool iscolliding; - private bool m_isSelected; - - internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - - private bool m_throttleUpdates; - private int throttleCounter; - public int m_interpenetrationcount { get; private set; } - internal float m_collisionscore; - public int m_roundsUnderMotionThreshold { get; private set; } - private int m_crossingfailures; + public float m_collisionscore; + private int m_colliderfilter = 0; - public bool outofBounds { get; private set; } - private float m_density = 10.000006836f; // Aluminum g/cm3; + public IntPtr collide_geom; // for objects: geom if single prim space it linkset - public bool _zeroFlag { get; private set; } + private float m_density; + private byte m_shapetype; + public bool _zeroFlag; private bool m_lastUpdateSent; - public IntPtr Body = IntPtr.Zero; + public IntPtr Body; + private Vector3 _target_velocity; - private d.Mass pMass; - private int m_eventsubscription; - private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + public Vector3 m_OBBOffset; + public Vector3 m_OBB; + public float primOOBradiusSQ; - /// - /// Signal whether there were collisions on the previous frame, so we know if we need to send the - /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision. - /// - /// - /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself. - /// - private bool m_collisionsOnPreviousFrame; + private bool m_hasOBB = true; + + private float m_physCost; + private float m_streamCost; + + public d.Mass primdMass; // prim inertia information on it's own referencial + float primMass; // prim own mass + float primVolume; // prim own volume; + float _mass; // object mass acording to case + + public int givefakepos; + private Vector3 fakepos; + public int givefakeori; + private Quaternion fakeori; - private IntPtr m_linkJoint = IntPtr.Zero; + private int m_eventsubscription; + private int m_cureventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = null; + private bool SentEmptyCollisionsEvent; - internal volatile bool childPrim; + public volatile bool childPrim; - private ODEDynamics m_vehicle; + public ODEDynamics m_vehicle; internal int m_material = (int)Material.Wood; + private float mu; + private float bounce; - public OdePrim( - String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical // this is not reliable for internal use { - Name = primName; - m_vehicle = new ODEDynamics(); - //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); - - if (!pos.IsFinite()) + get { return m_fakeisphysical; } + set { - pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), - parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); - } - _position = pos; - m_taintposition = pos; - PID_D = parent_scene.bodyPIDD; - PID_G = parent_scene.bodyPIDG; - m_density = parent_scene.geomDefaultDensity; - // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; - body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + m_fakeisphysical = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing - prim_geom = IntPtr.Zero; + if (!value) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + AddChange(changes.Physical, value); + } + } - if (!pos.IsFinite()) + public override bool IsVolumeDtc + { + get { return m_fakeisVolumeDetect; } + set { - size = new Vector3(0.5f, 0.5f, 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + m_fakeisVolumeDetect = value; + AddChange(changes.VolumeDtc, value); } + } - if (size.X <= 0) size.X = 0.01f; - if (size.Y <= 0) size.Y = 0.01f; - if (size.Z <= 0) size.Z = 0.01f; - - _size = size; - m_taintsize = _size; + public override bool Phantom // this is not reliable for internal use + { + get { return m_fakeisphantom; } + set + { + m_fakeisphantom = value; + AddChange(changes.Phantom, value); + } + } - if (!QuaternionIsFinite(rotation)) + public override bool Building // this is not reliable for internal use + { + get { return m_building; } + set { - rotation = Quaternion.Identity; - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + if (value) + m_building = true; + AddChange(changes.building, value); } + } - _orientation = rotation; - m_taintrot = _orientation; - _pbs = pbs; + public override void getContactData(ref ContactData cdata) + { + cdata.mu = mu; + cdata.bounce = bounce; - _parent_scene = parent_scene; - m_targetSpace = (IntPtr)0; + // cdata.softcolide = m_softcolide; + cdata.softcolide = false; - if (pos.Z < 0) + if (m_isphysical) { - IsPhysical = false; + ODEDynamics veh; + if (_parent != null) + veh = ((OdePrim)_parent).m_vehicle; + else + veh = m_vehicle; + + if (veh != null && veh.Type != Vehicle.TYPE_NONE) + cdata.mu *= veh.FrictionFactor; +// cdata.mu *= 0; } - else + } + + public override float PhysicsCost + { + get { - IsPhysical = pisPhysical; - // If we're physical, we need to be in the master space for now. - // linksets *should* be in a space together.. but are not currently - if (IsPhysical) - m_targetSpace = _parent_scene.space; + return m_physCost; } + } - m_taintadd = true; - m_assetFailed = false; - _parent_scene.AddPhysicsActorTaint(this); + public override float StreamCost + { + get + { + return m_streamCost; + } } public override int PhysicsActorType { - get { return (int) ActorTypes.Prim; } + get { return (int)ActorTypes.Prim; } set { return; } } @@ -301,6 +307,23 @@ namespace OpenSim.Region.Physics.OdePlugin set { return; } } + public override uint LocalID + { + get { return m_localID; } + set { m_localID = value; } + } + + public override PhysicsActor ParentActor + { + get + { + if (childPrim) + return _parent; + else + return (PhysicsActor)this; + } + } + public override bool Grabbed { set { return; } @@ -310,2383 +333,3063 @@ namespace OpenSim.Region.Physics.OdePlugin { set { - // This only makes the object not collidable if the object - // is physical or the object is modified somehow *IN THE FUTURE* - // without this, if an avatar selects prim, they can walk right - // through it while it's selected - m_collisionscore = 0; + if (value) + m_isSelected = value; // if true set imediatly to stop moves etc + AddChange(changes.Selected, value); + } + } - if ((IsPhysical && !_zeroFlag) || !value) + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + if (value) { - m_taintselected = value; - _parent_scene.AddPhysicsActorTaint(this); + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; } else { - m_taintselected = value; - m_isSelected = value; + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; } - if (m_isSelected) - disableBodySoft(); + if (m_colliderfilter == 0) + m_iscolliding = false; + else + m_iscolliding = true; } } - /// - /// Set a new geometry for this prim. - /// - /// - private void SetGeom(IntPtr geom) + public override bool CollidingGround { - prim_geom = geom; -//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + + public override bool ThrottleUpdates {get;set;} + + public override bool Stopped + { + get { return _zeroFlag; } + } - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + public override Vector3 Position + { + get + { + if (givefakepos > 0) + return fakepos; + else + return _position; + } - _parent_scene.geom_name_map[prim_geom] = Name; - _parent_scene.actor_name_map[prim_geom] = this; + set + { + fakepos = value; + givefakepos++; + AddChange(changes.Position, value); + } + } - if (childPrim) + public override Vector3 Size + { + get { return _size; } + set { - if (_parent != null && _parent is OdePrim) + if (value.IsFinite()) + { + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype); + } + else { - OdePrim parent = (OdePrim)_parent; -//Console.WriteLine("SetGeom calls ChildSetGeom"); - parent.ChildSetGeom(this); + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); } } - //m_log.Warn("Setting Geom to: " + prim_geom); } - private void enableBodySoft() + public override float Mass { - if (!childPrim) + get { return primMass; } + } + + public override Vector3 Force + { + get { return m_force; } + set { - if (IsPhysical && Body != IntPtr.Zero) + if (value.IsFinite()) { - d.BodyEnable(Body); - if (m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Enable(Body, _parent_scene); + AddChange(changes.Force, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); } - - m_disabled = false; } } - private void disableBodySoft() + public override void SetVolumeDetect(int param) { - m_disabled = true; + m_fakeisVolumeDetect = (param != 0); + AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); + } - if (IsPhysical && Body != IntPtr.Zero) + public override Vector3 GeometricCenter + { + // this is not real geometric center but a average of positions relative to root prim acording to + // http://wiki.secondlife.com/wiki/llGetGeometricCenter + // ignoring tortured prims details since sl also seems to ignore + // so no real use in doing it on physics + get { - d.BodyDisable(Body); + return Vector3.Zero; } } - /// - /// Make a prim subject to physics. - /// - private void enableBody() + public override Vector3 CenterOfMass { - // Don't enable this body if we're a child prim - // this should be taken care of in the parent function not here - if (!childPrim) + get { - // Sets the geom to a body - Body = d.BodyCreate(_parent_scene.world); + lock (_parent_scene.OdeLock) + { + d.Vector3 dtmp; + if (!childPrim && Body != IntPtr.Zero) + { + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; + + Vector3 Ptot = m_OBBOffset * q; + dtmp = d.GeomGetPosition(prim_geom); + Ptot.X += dtmp.X; + Ptot.Y += dtmp.Y; + Ptot.Z += dtmp.Z; + + // if(childPrim) we only know about physical linksets + return Ptot; +/* + float tmass = _mass; + Ptot *= tmass; - setMass(); - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.BodySetQuaternion(Body, ref myrot); - d.GeomSetBody(prim_geom, Body); - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + float m; - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + foreach (OdePrim prm in childrenPrim) + { + m = prm._mass; + Ptot += prm.CenterOfMass * m; + tmass += m; + } - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode (Body, false); + if (tmass == 0) + tmass = 0; + else + tmass = 1.0f / tmass; - m_interpenetrationcount = 0; - m_collisionscore = 0; - m_disabled = false; + Ptot *= tmass; + return Ptot; +*/ + } + else + return _position; + } + } + } - // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) + public override Vector3 OOBsize + { + get { - createAMotor(m_angularlock); + return m_OBB; } - if (m_vehicle.Type != Vehicle.TYPE_NONE) + } + + public override Vector3 OOBoffset + { + get { - m_vehicle.Enable(Body, _parent_scene); + return m_OBBOffset; } + } - _parent_scene.ActivatePrim(this); + public override float OOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } } - } - #region Mass Calculation + public override PrimitiveBaseShape Shape + { + set + { +// AddChange(changes.Shape, value); + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype); + } + } - private float CalculateMass() + public override byte PhysicsShapeType { - float volume = _size.X * _size.Y * _size.Z; // default - float tmp; + get + { + return m_shapetype; + } + set + { + m_shapetype = value; + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); + } + } - float returnMass = 0; - float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - - switch (_pbs.ProfileShape) + public override Vector3 Velocity + { + get + { + if (_zeroFlag) + return Vector3.Zero; + return _velocity; + } + set { - case ProfileShape.Square: - // default box + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); + } - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; + } + } - case HollowShape.Circle: + public override Vector3 Torque + { + get + { + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; - hollowVolume *= 0.78539816339f; - break; + return _torque; + } - case HollowShape.Triangle: + set + { + if (value.IsFinite()) + { + AddChange(changes.Torque, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); + } + } + } - hollowVolume *= (0.5f * .5f); - break; + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get + { + if (givefakeori > 0) + return fakeori; + else + + return _orientation; + } + set + { + if (QuaternionIsFinite(value)) + { + fakeori = value; + givefakeori++; + + value.Normalize(); + + AddChange(changes.Orientation, value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); + + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.AngVelocity, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + AddChange(changes.Buoyancy,value); + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + AddChange(changes.PIDTarget,value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive + { + set + { + AddChange(changes.PIDActive,value); + } + } + + public override float PIDTau + { + set + { + float tmp = 0; + if (value > 0) + { + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); + if (value < mint) + tmp = mint; + else + tmp = value; + } + AddChange(changes.PIDTau,tmp); + } + } + + public override float PIDHoverHeight + { + set + { + AddChange(changes.PIDHoverHeight,value); + } + } + public override bool PIDHoverActive + { + set + { + AddChange(changes.PIDHoverActive, value); + } + } + + public override PIDHoverType PIDHoverType + { + set + { + AddChange(changes.PIDHoverType,value); + } + } + + public override float PIDHoverTau + { + set + { + float tmp =0; + if (value > 0) + { + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); + if (value < mint) + tmp = mint; + else + tmp = value; + } + AddChange(changes.PIDHoverTau, tmp); + } + } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + public override int VehicleType + { + // we may need to put a fake on this + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + AddChange(changes.VehicleType, value); + } + } + + public override void VehicleFloatParam(int param, float value) + { + strVehicleFloatParam fp = new strVehicleFloatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleFloatParam, fp); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + strVehicleVectorParam fp = new strVehicleVectorParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); + } + + public override void VehicleRotationParam(int param, Quaternion value) + { + strVehicleQuatParam fp = new strVehicleQuatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleRotationParam, fp); + } + + public override void VehicleFlags(int param, bool value) + { + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); + } + + public override void SetVehicle(object vdata) + { + AddChange(changes.SetVehicle, vdata); + } + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + if(pushforce) + AddChange(changes.AddForce, force); + else // a impulse + AddChange(changes.AddForce, force * m_invTimeStep); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { +// if(pushforce) for now applyrotationimpulse seems more happy applied as a force + AddChange(changes.AddAngForce, force); +// else // a impulse +// AddChange(changes.AddAngForce, force * m_invTimeStep); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + if (m_outbounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + m_lastposition = _position; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + if(Body != IntPtr.Zero) + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + if (prim_geom != IntPtr.Zero) + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + + m_outbounds = false; + changeDisable(false); + base.RequestPhysicsterseUpdate(); + } + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + mu = _parent_scene.m_materialContactsData[pMaterial].mu; + bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public void setPrimForRemoval() + { + AddChange(changes.Remove, null); + } + + public override void link(PhysicsActor obj) + { + AddChange(changes.Link, obj); + } + + public override void delink() + { + AddChange(changes.DeLink, null); + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; +// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axis); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + m_cureventsubscription = 0; + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + SentEmptyCollisionsEvent = false; + } + + public override void UnSubscribeEvents() + { + if (CollisionEventsThisFrame != null) + { + CollisionEventsThisFrame.Clear(); + CollisionEventsThisFrame = null; + } + m_eventsubscription = 0; + _parent_scene.RemoveCollisionEventReporting(this); + } + + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); +// if(CollisionEventsThisFrame.Count < 32) + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + if (m_cureventsubscription < m_eventsubscription) + return; + + m_cureventsubscription = 0; + + int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + + if (!SentEmptyCollisionsEvent || ncolisions > 0) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (ncolisions == 0) + { + SentEmptyCollisionsEvent = true; + _parent_scene.RemoveCollisionEventReporting(this); + } + else + { + SentEmptyCollisionsEvent = false; + CollisionEventsThisFrame.Clear(); + } + } + } + + internal void AddCollisionFrameTime(int t) + { + if (m_cureventsubscription < 50000) + m_cureventsubscription += t; + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) + { + Name = primName; + LocalID = plocalID; + + m_vehicle = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); + } + _position = pos; + givefakepos = 0; + + m_timeStep = parent_scene.ODE_STEPSIZE; + m_invTimeStep = 1f / m_timeStep; + + m_density = parent_scene.geomDefaultDensity; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; + Body = IntPtr.Zero; + + if (!size.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + } + + _orientation = rotation; + givefakeori = 0; + + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + m_fakeisVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_colliderfilter = 0; + m_NoColide = false; + + _triMeshData = IntPtr.Zero; + + m_shapetype = _shapeType; + + m_lastdoneSelected = false; + m_isSelected = false; + m_delaySelect = false; + + m_isphantom = pisPhantom; + m_fakeisphantom = pisPhantom; + + mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + m_building = true; // control must set this to false when done + + _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + } + + private void UpdateCollisionCatFlags() + { + if(m_isphysical && m_disabled) + { + m_collisionCategories = 0; + m_collisionFlags = 0; + } + + else if (m_isSelected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = 0; + } + + else if (m_isVolumeDetect) + { + m_collisionCategories = CollisionCategories.VolumeDtc; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; + else + m_collisionFlags = 0; + } + else if (m_isphantom) + { + m_collisionCategories = CollisionCategories.Phantom; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionFlags = m_default_collisionFlagsPhysical; + else + m_collisionFlags = m_default_collisionFlagsNotPhysical; + } + } + + private void ApplyCollisionCatFlags() + { + if (prim_geom != IntPtr.Zero) + { + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + { + if (m_isphysical && m_disabled) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = 0; + } + else + { + // preserve some + if (prm.m_isSelected) + { + prm.m_collisionCategories = CollisionCategories.Selected; + prm.m_collisionFlags = 0; + } + else if (prm.m_isVolumeDetect) + { + prm.m_collisionCategories = CollisionCategories.VolumeDtc; + if (m_isphysical) + prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; + else + prm.m_collisionFlags = 0; + } + else if (prm.m_isphantom) + { + prm.m_collisionCategories = CollisionCategories.Phantom; + if (m_isphysical) + prm.m_collisionFlags = CollisionCategories.Land; + else + prm.m_collisionFlags = 0; + } + else + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; } } - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + if (prm.prim_geom != IntPtr.Zero) { - //a tube - - volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume*tmp*tmp; - - if (hollowAmount > 0.0) + if (prm.m_NoColide) { - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - hollowVolume *= 0.78539816339f;; - break; - - case HollowShape.Triangle: - hollowVolume *= 0.5f * 0.5f; - break; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); } } + } + } - break; + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, 0); + d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); + } + } + } + } - case ProfileShape.Circle: + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.78539816339f; // elipse base + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; + int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; + if (axisnum <= 0) + return; - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; + // stop it + d.BodySetTorque(Body, 0, 0, 0); + d.BodySetAngularVel(Body, 0, 0, 0); - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { + d.JointSetAMotorMode(Amotor, 0); - // calculate the hollow volume by it's shape compared to the prim shape - hollowVolume *= hollowAmount; + d.JointSetAMotorNumAxes(Amotor, axisnum); - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; + // get current orientation to lock - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; + d.Quaternion dcur = d.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; + Vector3 ax; - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; + int i = 0; + int j = 0; + if (axis.X == 0) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + // ODE should do this with axis relative to body 1 but seems to fail + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); + i++; + j = 256; // move to next axis set + } + + if (axis.Y == 0) + { + ax = (new Vector3(0, 1, 0)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + i++; + j += 256; + } + + if (axis.Z == 0) + { + ax = (new Vector3(0, 0, 1)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + } + } - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - case ProfileShape.HalfCircle: - if (_pbs.PathCurve == (byte)Extrusion.Curve1) + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) { - volume *= 0.52359877559829887307710723054658f; + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); } - break; + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + } - case ProfileShape.EquilateralTriangle: + UpdatePrimBodyData(); + _parent_scene.actor_name_map[prim_geom] = this; - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.32475953f; +/* +// debug + d.AABB aabb; + d.GeomGetAABB(prim_geom, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if( x > 60.0f || y > 60.0f || z > 60.0f) + m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", + Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); + else if (x < 0.001f || y < 0.001f || z < 0.001f) + m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", + Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); + +// +*/ - if (hollowAmount > 0.0) - { + } + else + m_log.Warn("Setting bad Geom"); + } - // calculate the hollow volume by it's shape compared to the prim shape - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; + private bool GetMeshGeom() + { + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + IMesh mesh = m_mesh; - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; + if (mesh == null) + return false; - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1}", + Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh"); - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.32475953f; - volume *= 0.01f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); + m_hasOBB = false; + m_OBBOffset = Vector3.Zero; + m_OBB = _size * 0.5f; - if (hollowAmount > 0.0) - { + m_physCost = 0.1f; + m_streamCost = 1.0f; + + _parent_scene.mesher.ReleaseMesh(mesh); + m_meshState = MeshState.MeshFailed; + m_mesh = null; + return false; + } - hollowVolume *= hollowAmount; + IntPtr geo = IntPtr.Zero; - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; + try + { + _triMeshData = d.GeomTriMeshDataCreate(); - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); - case HollowShape.Circle: + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + if (_triMeshData != IntPtr.Zero) + { + try + { + d.GeomTriMeshDataDestroy(_triMeshData); + } + catch + { + } + } + _triMeshData = IntPtr.Zero; + + m_hasOBB = false; + m_OBBOffset = Vector3.Zero; + m_OBB = _size * 0.5f; + m_physCost = 0.1f; + m_streamCost = 1.0f; + + _parent_scene.mesher.ReleaseMesh(mesh); + m_meshState = MeshState.MeshFailed; + m_mesh = null; + return false; + } - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; + m_physCost = 0.0013f * (float)indexCount; + // todo + m_streamCost = 1.0f; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } + SetGeom(geo); + + return true; + } + + private void CreateGeom() + { + bool hasMesh = false; + + m_NoColide = false; + + if ((m_meshState & MeshState.FailMask) != 0) + m_NoColide = true; + + else if(m_mesh != null) + { + if (GetMeshGeom()) + hasMesh = true; + else + m_NoColide = true; + } + + + if (!hasMesh) + { + IntPtr geo = IntPtr.Zero; + + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + try + { + geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return; + } + } + else + {// do it as a box + try + { + geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + m_physCost = 0.1f; + m_streamCost = 1.0f; + SetGeom(geo); + } + } + + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.actor_name_map.Remove(prim_geom); + + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); + } + + prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; + m_targetSpace = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + + lock (m_meshlock) + { + if (m_mesh != null) + { + _parent_scene.mesher.ReleaseMesh(m_mesh); + m_mesh = null; + } + } + + Body = IntPtr.Zero; + m_hasOBB = false; + } + + //sets non physical prim m_targetSpace to right space in spaces grid for static prims + // should only be called for non physical prims unless they are becoming non physical + private void SetInStaticSpace(OdePrim prim) + { + IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); + prim.m_targetSpace = targetSpace; + collide_geom = IntPtr.Zero; + } + + public void enableBodySoft() + { + m_disabled = false; + if (!childPrim && !m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + + d.BodyEnable(Body); + } + } + resetCollisionAccounting(); + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_isSelected) + m_collisionFlags = CollisionCategories.Selected; + else + m_collisionCategories = 0; + m_collisionFlags = 0; + ApplyCollisionCatFlags(); + d.BodyDisable(Body); + } + } + } + + private void MakeBody() + { + if (!m_isphysical) // only physical get bodies + return; + + if (childPrim) // child prims don't get bodies; + return; + + if (m_building) + return; + + if (prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); + return; + } + + if (Body != IntPtr.Zero) + { + DestroyBody(); + m_log.Warn("[PHYSICS]: MakeBody called having a body"); + } + + if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); + } + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + Body = d.BodyCreate(_parent_scene.world); + + objdmass = primdMass; + + // rotate inertia + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + // set the body rotation + d.BodySetRotation(Body, ref mymat); + + // recompute full object inertia if needed + if (childrenPrim.Count > 0) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; + Vector3 rcm; + + rcm.X = _position.X; + rcm.Y = _position.Y; + rcm.Z = _position.Z; + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); + continue; + } + + tmpdmass = prm.primdMass; + + // apply prim current rotation to inertia + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + quat.W = prm._orientation.W; + d.RfromQ(out mat, ref quat); + d.MassRotate(ref tmpdmass, ref mat); + + Vector3 ppos = prm._position; + ppos.X -= rcm.X; + ppos.Y -= rcm.Y; + ppos.Z -= rcm.Z; + // refer inertia to root prim center of mass position + d.MassTranslate(ref tmpdmass, + ppos.X, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats + + if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); } - break; - default: - break; - } + d.GeomClearOffset(prm.prim_geom); + d.GeomSetBody(prm.prim_geom, Body); + prm.Body = Body; + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.X = -myrot.X; + myrot.Y = -myrot.Y; + myrot.Z = -myrot.Z; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; - float taperX1; - float taperY1; - float taperX; - float taperY; - float pathBegin; - float pathEnd; - float profileBegin; - float profileEnd; + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); - if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + d.BodySetDamping(Body, .005f, .005f); + + if (m_targetSpace != IntPtr.Zero) { - taperX1 = _pbs.PathScaleX * 0.01f; - if (taperX1 > 1.0f) - taperX1 = 2.0f - taperX1; - taperX = 1.0f - taperX1; + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } - taperY1 = _pbs.PathScaleY * 0.01f; - if (taperY1 > 1.0f) - taperY1 = 2.0f - taperY1; - taperY = 1.0f - taperY1; + if (childrenPrim.Count == 0) + { + collide_geom = prim_geom; + m_targetSpace = _parent_scene.ActiveSpace; } else { - taperX = _pbs.PathTaperX * 0.01f; - if (taperX < 0.0f) - taperX = -taperX; - taperX1 = 1.0f - taperX; + m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); + d.HashSpaceSetLevels(m_targetSpace, -2, 8); + d.SpaceSetSublevel(m_targetSpace, 3); + d.SpaceSetCleanup(m_targetSpace, false); - taperY = _pbs.PathTaperY * 0.01f; - if (taperY < 0.0f) - taperY = -taperY; - taperY1 = 1.0f - taperY; + d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(m_targetSpace, 0); + collide_geom = m_targetSpace; } - volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + d.SpaceAdd(m_targetSpace, prim_geom); - pathBegin = (float)_pbs.PathBegin * 2.0e-5f; - pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; - volume *= (pathEnd - pathBegin); + if (m_delaySelect) + { + m_isSelected = true; + m_delaySelect = false; + } -// this is crude aproximation - profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; - profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; - volume *= (profileEnd - profileBegin); + m_collisionscore = 0; - returnMass = m_density * volume; + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); - if (returnMass <= 0) - returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. -// else if (returnMass > _parent_scene.maximumMassObject) -// returnMass = _parent_scene.maximumMassObject; + _parent_scene.addActivePrim(this); - // Recursively calculate mass - bool HasChildPrim = false; lock (childrenPrim) { - if (childrenPrim.Count > 0) + foreach (OdePrim prm in childrenPrim) { - HasChildPrim = true; - } - } - - if (HasChildPrim) - { - OdePrim[] childPrimArr = new OdePrim[0]; + if (prm.prim_geom == IntPtr.Zero) + continue; - lock (childrenPrim) - childPrimArr = childrenPrim.ToArray(); + Vector3 ppos = prm._position; + d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position - for (int i = 0; i < childPrimArr.Length; i++) - { - if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) - returnMass += childPrimArr[i].CalculateMass(); - // failsafe, this shouldn't happen but with OpenSim, you never know :) - if (i > 256) - break; - } - } + if (prm.m_targetSpace != m_targetSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = m_targetSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } - if (returnMass > _parent_scene.maximumMassObject) - returnMass = _parent_scene.maximumMassObject; + prm.m_collisionscore = 0; - return returnMass; - } + if(!m_disabled) + prm.m_disabled = false; - #endregion + _parent_scene.addActivePrim(prm); + } + } - private void setMass() - { - if (Body != (IntPtr) 0) + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) { - float newmass = CalculateMass(); + createAMotor(m_angularlock); + } - //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString()); - d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z); - d.BodySetMass(Body, ref pMass); + if (m_isSelected || m_disabled) + { + d.BodyDisable(Body); } + else + { + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } + _parent_scene.addActiveGroups(this); } - /// - /// Stop a prim from being subject to physics. - /// - internal void disableBody() + private void DestroyBody() { - //this kills the body so things like 'mesh' can re-create it. - lock (this) + if (Body != IntPtr.Zero) { + _parent_scene.remActivePrim(this); + + collide_geom = IntPtr.Zero; + + if (m_disabled) + m_collisionCategories = 0; + else if (m_isSelected) + m_collisionCategories = CollisionCategories.Selected; + else if (m_isVolumeDetect) + m_collisionCategories = CollisionCategories.VolumeDtc; + else if (m_isphantom) + m_collisionCategories = CollisionCategories.Phantom; + else + m_collisionCategories = CollisionCategories.Geom; + + m_collisionFlags = 0; + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + } + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + if (!childPrim) { - if (Body != IntPtr.Zero) + lock (childrenPrim) { - _parent_scene.DeactivatePrim(this); - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (prm.m_isSelected) + prm.m_collisionCategories = CollisionCategories.Selected; + else if (prm.m_isVolumeDetect) + prm.m_collisionCategories = CollisionCategories.VolumeDtc; + else if (prm.m_isphantom) + prm.m_collisionCategories = CollisionCategories.Phantom; + else + prm.m_collisionCategories = CollisionCategories.Geom; - d.BodyDestroy(Body); - lock (childrenPrim) - { - if (childrenPrim.Count > 0) + prm.m_collisionFlags = 0; + + if (prm.prim_geom != IntPtr.Zero) { - foreach (OdePrim prm in childrenPrim) + if (prm.m_NoColide) { - _parent_scene.DeactivatePrim(prm); - prm.Body = IntPtr.Zero; + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); + } + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); } + prm.Body = IntPtr.Zero; + prm._mass = prm.primMass; + prm.m_collisionscore = 0; } - Body = IntPtr.Zero; } + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + _parent_scene.remActiveGroup(this); + d.BodyDestroy(Body); } - else - { - _parent_scene.DeactivatePrim(this); - - m_collisionCategories &= ~CollisionCategories.Body; - m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - - Body = IntPtr.Zero; - } + Body = IntPtr.Zero; } - - m_disabled = true; + _mass = primMass; m_collisionscore = 0; } - private static Dictionary m_MeshToTriMeshMap = new Dictionary(); - - private void setMesh(OdeScene parent_scene, IMesh mesh) + private void FixInertia(Vector3 NewPos,Quaternion newrot) { -// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh); - - // This sleeper is there to moderate how long it takes between - // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); - //Thread.Sleep(10); + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; - //Kill Body so that mesh can re-make the geom - if (IsPhysical && Body != IntPtr.Zero) - { - if (childPrim) - { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this); - } - } - else - { - disableBody(); - } - } + d.BodyGetMass(Body, out tmpdmass); + objdmass = tmpdmass; - IntPtr vertices, indices; - int vertexCount, indexCount; - int vertexStride, triStride; - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage - m_expectedCollisionContacts = indexCount; - mesh.releaseSourceMeshData(); // free up the original mesh data to save memory + d.Vector3 dobjpos; + d.Vector3 thispos; - // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at - // the same time. - lock (m_MeshToTriMeshMap) - { - if (m_MeshToTriMeshMap.ContainsKey(mesh)) - { - _triMeshData = m_MeshToTriMeshMap[mesh]; - } - else - { - _triMeshData = d.GeomTriMeshDataCreate(); - - d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); - d.GeomTriMeshDataPreprocess(_triMeshData); - m_MeshToTriMeshMap[mesh] = _triMeshData; - } - } + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); -// _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { - SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); - } - catch (AccessViolationException) - { - m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name); - return; - } + // get prim own inertia in its local frame + tmpdmass = primdMass; - // if (IsPhysical && Body == (IntPtr) 0) - // { - // Recreate the body - // m_interpenetrationcount = 0; - // m_collisionscore = 0; + // transform to object frame + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); - // enableBody(); - // } - } + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); - internal void ProcessTaints() - { -#if SPAM -Console.WriteLine("ZProcessTaints for " + Name); -#endif + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); - // This must be processed as the very first taint so that later operations have a prim_geom to work with - // if this is a new prim. - if (m_taintadd) - changeadd(); + // back prim own inertia + tmpdmass = primdMass; - if (!_position.ApproxEquals(m_taintposition, 0f)) - changemove(); + // update to new position and orientation + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); - if (m_taintrot != _orientation) - { - if (childPrim && IsPhysical) // For physical child prim... - { - rotate(); - // KF: ODE will also rotate the parent prim! - // so rotate the root back to where it was - OdePrim parent = (OdePrim)_parent; - parent.rotate(); - } - else - { - //Just rotate the prim - rotate(); - } - } - - if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent)) - changePhysicsStatus(); + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); - if (!_size.ApproxEquals(m_taintsize, 0f)) - changesize(); + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); - if (m_taintshape) - changeshape(); + d.MassAdd(ref objdmass, ref tmpdmass); - if (m_taintforce) - changeAddForce(); + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); - if (m_taintaddangularforce) - changeAddAngularForce(); + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } - if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f)) - changeSetTorque(); - if (m_taintdisable) - changedisable(); - if (m_taintselected != m_isSelected) - changeSelectedStatus(); + private void FixInertia(Vector3 NewPos) + { + d.Matrix3 primmat = new d.Matrix3(); + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Mass primmass = new d.Mass { }; - if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) - changevelocity(); + d.Vector3 dobjpos; + d.Vector3 thispos; - if (m_taintparent != _parent) - changelink(); + d.BodyGetMass(Body, out objdmass); - if (m_taintCollidesWater != m_collidesWater) - changefloatonwater(); + // get prim own inertia in its local frame + primmass = primdMass; + // transform to object frame + primmat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref primmass, ref primmat); - if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) - changeAngularLock(); - } + tmpdmass = primmass; - /// - /// Change prim in response to an angular lock taint. - /// - private void changeAngularLock() - { - // do we have a Physical object? - if (Body != IntPtr.Zero) - { - //Check that we have a Parent - //If we have a parent then we're not authorative here - if (_parent == null) - { - if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) - { - //d.BodySetFiniteRotationMode(Body, 0); - //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); - createAMotor(m_taintAngularLock); - } - else - { - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - } - } - } + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); - // Store this for later in case we get turned into a separate body - m_angularlock = m_taintAngularLock; - } + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); - /// - /// Change prim in response to a link taint. - /// - private void changelink() - { - // If the newly set parent is not null - // create link - if (_parent == null && m_taintparent != null) - { - if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) - { - OdePrim obj = (OdePrim)m_taintparent; - //obj.disableBody(); -//Console.WriteLine("changelink calls ParentPrim"); - obj.AddChildPrim(this); + // update to new position + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); - /* - if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body) - { - _linkJointGroup = d.JointGroupCreate(0); - m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); - d.JointAttach(m_linkJoint, obj.Body, Body); - d.JointSetFixed(m_linkJoint); - } - */ - } - } - // If the newly set parent is null - // destroy link - else if (_parent != null && m_taintparent == null) - { -//Console.WriteLine(" changelink B"); - - if (_parent is OdePrim) - { - OdePrim obj = (OdePrim)_parent; - obj.ChildDelink(this); - childPrim = false; - //_parent = null; - } - - /* - if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) - d.JointGroupDestroy(_linkJointGroup); - - _linkJointGroup = (IntPtr)0; - m_linkJoint = (IntPtr)0; - */ - } - - _parent = m_taintparent; - m_taintPhysics = IsPhysical; - } + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref primmass, + thispos.X, + thispos.Y, + thispos.Z); - /// - /// Add a child prim to this parent prim. - /// - /// Child prim - private void AddChildPrim(OdePrim prim) - { - if (LocalID == prim.LocalID) - return; + d.MassAdd(ref objdmass, ref primmass); - if (Body == IntPtr.Zero) + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) { - Body = d.BodyCreate(_parent_scene.world); - setMass(); + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); } - lock (childrenPrim) - { - if (childrenPrim.Contains(prim)) - return; + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); -// m_log.DebugFormat( -// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID); + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); - childrenPrim.Add(prim); + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } - foreach (OdePrim prm in childrenPrim) - { - d.Mass m2; - d.MassSetZero(out m2); - d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z); + private void FixInertia(Quaternion newrot) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); - d.Quaternion quat = new d.Quaternion(); - quat.W = prm._orientation.W; - quat.X = prm._orientation.X; - quat.Y = prm._orientation.Y; - quat.Z = prm._orientation.Z; + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Vector3 dobjpos; + d.Vector3 thispos; - d.Matrix3 mat = new d.Matrix3(); - d.RfromQ(out mat, ref quat); - d.MassRotate(ref m2, ref mat); - d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z); - d.MassAdd(ref pMass, ref m2); - } + d.BodyGetMass(Body, out objdmass); - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories |= CollisionCategories.Body; - prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + // get prim own inertia in its local frame + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + // transform to object frame + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); -//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name); - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); - d.Quaternion quat = new d.Quaternion(); - quat.W = prm._orientation.W; - quat.X = prm._orientation.X; - quat.Y = prm._orientation.Y; - quat.Z = prm._orientation.Z; + // update to new orientation + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); - d.Matrix3 mat = new d.Matrix3(); - d.RfromQ(out mat, ref quat); - if (Body != IntPtr.Zero) - { - d.GeomSetBody(prm.prim_geom, Body); - prm.childPrim = true; - d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); - //d.GeomSetOffsetPosition(prim.prim_geom, - // (Position.X - prm.Position.X) - pMass.c.X, - // (Position.Y - prm.Position.Y) - pMass.c.Y, - // (Position.Z - prm.Position.Z) - pMass.c.Z); - d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); - //d.GeomSetOffsetRotation(prm.prim_geom, ref mat); - d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); - d.BodySetMass(Body, ref pMass); - } - else - { - m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name); - } + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); - prm.m_interpenetrationcount = 0; - prm.m_collisionscore = 0; - prm.m_disabled = false; + d.MassAdd(ref objdmass, ref tmpdmass); - // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) - { - prm.createAMotor(m_angularlock); - } - prm.Body = Body; - _parent_scene.ActivatePrim(prm); - } - - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); - -//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); -//Console.WriteLine(" Post GeomSetCategoryBits 2"); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - - d.Quaternion quat2 = new d.Quaternion(); - quat2.W = _orientation.W; - quat2.X = _orientation.X; - quat2.Y = _orientation.Y; - quat2.Z = _orientation.Z; - - d.Matrix3 mat2 = new d.Matrix3(); - d.RfromQ(out mat2, ref quat2); - d.GeomSetBody(prim_geom, Body); - d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z); - //d.GeomSetOffsetPosition(prim.prim_geom, - // (Position.X - prm.Position.X) - pMass.c.X, - // (Position.Y - prm.Position.Y) - pMass.c.Y, - // (Position.Z - prm.Position.Z) - pMass.c.Z); - //d.GeomSetOffsetRotation(prim_geom, ref mat2); - d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); - d.BodySetMass(Body, ref pMass); - - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - - m_interpenetrationcount = 0; - m_collisionscore = 0; - m_disabled = false; + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } - // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) - { - createAMotor(m_angularlock); - } + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); - d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } - if (m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Enable(Body, _parent_scene); - _parent_scene.ActivatePrim(this); - } - } + #region Mass Calculation - private void ChildSetGeom(OdePrim odePrim) + private void UpdatePrimBodyData() { -// m_log.DebugFormat( -// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); + primMass = m_density * primVolume; - //if (IsPhysical && Body != IntPtr.Zero) - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - //prm.childPrim = true; - prm.disableBody(); - //prm.m_taintparent = null; - //prm._parent = null; - //prm.m_taintPhysics = false; - //prm.m_disabled = true; - //prm.childPrim = false; - } - } + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; - disableBody(); + _mass = primMass; // just in case - // Spurious - Body == IntPtr.Zero after disableBody() -// if (Body != IntPtr.Zero) -// { -// _parent_scene.DeactivatePrim(this); -// } + d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z); - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { -//Console.WriteLine("ChildSetGeom calls ParentPrim"); - AddChildPrim(prm); - } - } - } + d.MassTranslate(ref primdMass, + m_OBBOffset.X, + m_OBBOffset.Y, + m_OBBOffset.Z); - private void ChildDelink(OdePrim odePrim) - { -// m_log.DebugFormat( -// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); + primOOBradiusSQ = m_OBB.LengthSquared(); - // Okay, we have a delinked child.. need to rebuild the body. - lock (childrenPrim) + if (_triMeshData != IntPtr.Zero) { - foreach (OdePrim prm in childrenPrim) - { - prm.childPrim = true; - prm.disableBody(); - //prm.m_taintparent = null; - //prm._parent = null; - //prm.m_taintPhysics = false; - //prm.m_disabled = true; - //prm.childPrim = false; - } - } + float pc = m_physCost; + float psf = primOOBradiusSQ; + psf *= 1.33f * .2f; + pc *= psf; + if (pc < 0.1f) + pc = 0.1f; - disableBody(); - - lock (childrenPrim) - { - //Console.WriteLine("childrenPrim.Remove " + odePrim); - childrenPrim.Remove(odePrim); + m_physCost = pc; } + else + m_physCost = 0.1f; - // Spurious - Body == IntPtr.Zero after disableBody() -// if (Body != IntPtr.Zero) -// { -// _parent_scene.DeactivatePrim(this); -// } - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { -//Console.WriteLine("ChildDelink calls ParentPrim"); - AddChildPrim(prm); - } - } + m_streamCost = 1.0f; } + #endregion + + /// - /// Change prim in response to a selection taint. + /// Add a child prim to this parent prim. /// - private void changeSelectedStatus() + /// Child prim + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) { - if (m_taintselected) + //Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); - - // We do the body disable soft twice because 'in theory' a collision could have happened - // in between the disabling and the collision properties setting - // which would wake the physical body up from a soft disabling and potentially cause it to fall - // through the ground. - - // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select - // just one part of the assembly, the rest of the assembly is non-selected and still simulating, - // so that causes the selected part to wake up and continue moving. - - // even if you select all parts of a jointed assembly, it is not guaranteed that the entire - // assembly will stop simulating during the selection, because of the lack of atomicity - // of select operations (their processing could be interrupted by a thread switch, causing - // simulation to continue before all of the selected object notifications trickle down to - // the physics engine). - - // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are - // selected and disabled. then, due to a thread switch, the selection processing is - // interrupted and the physics engine continues to simulate, so the last 50 items, whose - // selection was not yet processed, continues to simulate. this wakes up ALL of the - // first 50 again. then the last 50 are disabled. then the first 50, which were just woken - // up, start simulating again, which in turn wakes up the last 50. - - if (IsPhysical) - { - disableBodySoft(); - } + DestroyBody(); // for now we need to rebuil entire object on link change - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - - if (IsPhysical) + lock (childrenPrim) { - disableBodySoft(); - } - } - else - { - m_collisionCategories = CollisionCategories.Geom; - - if (IsPhysical) - m_collisionCategories |= CollisionCategories.Body; - - m_collisionFlags = m_default_collisionFlags; + // adopt the prim + if (!childrenPrim.Contains(prim)) + childrenPrim.Add(prim); - if (m_collidesLand) - m_collisionFlags |= CollisionCategories.Land; - if (m_collidesWater) - m_collisionFlags |= CollisionCategories.Water; + // see if this prim has kids and adopt them also + // should not happen for now + foreach (OdePrim prm in prim.childrenPrim) + { + if (!childrenPrim.Contains(prm)) + { + if (prm.Body != IntPtr.Zero) + { + if (prm.prim_geom != IntPtr.Zero) + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + if (prm.Body != prim.Body) + prm.DestroyBody(); // don't loose bodies around + prm.Body = IntPtr.Zero; + } - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + childrenPrim.Add(prm); + prm._parent = this; + } + } + } + //Remove old children from the prim + prim.childrenPrim.Clear(); - if (IsPhysical) + if (prim.Body != IntPtr.Zero) { - if (Body != IntPtr.Zero) - { - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0, 0, 0); - enableBodySoft(); - } + if (prim.prim_geom != IntPtr.Zero) + d.GeomSetBody(prim.prim_geom, IntPtr.Zero); + prim.DestroyBody(); // don't loose bodies around + prim.Body = IntPtr.Zero; } - } - resetCollisionAccounting(); - m_isSelected = m_taintselected; - }//end changeSelectedStatus + prim.childPrim = true; + prim._parent = this; + + MakeBody(); // full nasty reconstruction + } + } - internal void ResetTaints() + private void UpdateChildsfromgeom() { - m_taintposition = _position; - m_taintrot = _orientation; - m_taintPhysics = IsPhysical; - m_taintselected = m_isSelected; - m_taintsize = _size; - m_taintshape = false; - m_taintforce = false; - m_taintdisable = false; - m_taintVelocity = Vector3.Zero; + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.UpdateDataFromGeom(); + } } - /// - /// Create a geometry for the given mesh in the given target space. - /// - /// - /// If null, then a mesh is used that is based on the profile shape data. - private void CreateGeom(IntPtr m_targetSpace, IMesh mesh) + private void UpdateDataFromGeom() { -#if SPAM -Console.WriteLine("CreateGeom:"); -#endif - if (mesh != null) + if (prim_geom != IntPtr.Zero) { - setMesh(_parent_scene, mesh); + d.Quaternion qtmp; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + _orientation.W = qtmp.W; +/* +// Debug + float qlen = _orientation.Length(); + if (qlen > 1.01f || qlen < 0.99) + m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); +// +*/ + _orientation.Normalize(); + + d.Vector3 lpos = d.GeomGetPosition(prim_geom); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; } - else + } + + private void ChildDelink(OdePrim odePrim, bool remakebodies) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) // delinking the root prim { - if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) + OdePrim newroot = null; + lock (childrenPrim) { - if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) + if (childrenPrim.Count > 0) { - if (((_size.X / 2f) > 0f)) + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) { -// _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { -//Console.WriteLine(" CreateGeom 1"); - SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); - m_expectedCollisionContacts = 3; - } - catch (AccessViolationException) - { - m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); - return; - } - } - else - { -// _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { -//Console.WriteLine(" CreateGeom 2"); - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); - m_expectedCollisionContacts = 4; - } - catch (AccessViolationException) - { - m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); - return; - } + newroot.childrenPrim.Add(prm); } + childrenPrim.Clear(); } - else + if (newroot != null) { -// _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { -//Console.WriteLine(" CreateGeom 3"); - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); - m_expectedCollisionContacts = 4; - } - catch (AccessViolationException) - { - m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); - return; - } + newroot.childPrim = false; + newroot._parent = null; + if (remakebodies) + newroot.MakeBody(); } } - else + } + + else + { + lock (childrenPrim) { -// _parent_scene.waitForSpaceUnlock(m_targetSpace); - try - { -//Console.WriteLine(" CreateGeom 4"); - SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); - m_expectedCollisionContacts = 4; - } - catch (AccessViolationException) - { - m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); - return; - } + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + // odePrim.UpdateDataFromGeom(); + if (remakebodies) + odePrim.MakeBody(); } } + if (remakebodies) + MakeBody(); } - /// - /// Remove the existing geom from this prim. - /// - /// - /// If null, then a mesh is used that is based on the profile shape data. - /// true if the geom was successfully removed, false if it was already gone or the remove failed. - internal bool RemoveGeom() + protected void ChildRemove(OdePrim odePrim, bool reMakeBody) { - if (prim_geom != IntPtr.Zero) + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) { - try - { - _parent_scene.geom_name_map.Remove(prim_geom); - _parent_scene.actor_name_map.Remove(prim_geom); - d.GeomDestroy(prim_geom); - m_expectedCollisionContacts = 0; - prim_geom = IntPtr.Zero; - } - catch (System.AccessViolationException) + OdePrim newroot = null; + lock (childrenPrim) { - prim_geom = IntPtr.Zero; - m_expectedCollisionContacts = 0; - m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name); - - return false; + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + newroot.MakeBody(); + } } - - return true; + if (reMakeBody) + MakeBody(); + return; } else { - m_log.WarnFormat( - "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID); - - return false; + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + if (reMakeBody) + odePrim.MakeBody(); + } } + MakeBody(); } - /// - /// Add prim in response to an add taint. - /// - private void changeadd() - { -// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name); - - int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); - - if (targetspace == IntPtr.Zero) - targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); - - m_targetSpace = targetspace; - - IMesh mesh = null; - - if (_parent_scene.needsMeshing(_pbs)) - { - // Don't need to re-enable body.. it's done in SetMesh - mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); - // createmesh returns null when it's a shape that isn't a cube. - // m_log.Debug(m_localID); - if (mesh == null) - CheckMeshAsset(); - } - -#if SPAM -Console.WriteLine("changeadd 1"); -#endif - CreateGeom(m_targetSpace, mesh); - - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - if (IsPhysical && Body == IntPtr.Zero) - enableBody(); + #region changes - changeSelectedStatus(); - - m_taintadd = false; + private void changeadd() + { } - /// - /// Move prim in response to a move taint. - /// - private void changemove() + private void changeAngularLock(Vector3 newLock) { - if (IsPhysical) + // do we have a Physical object? + if (Body != IntPtr.Zero) { - if (!m_disabled && !m_taintremove && !childPrim) + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) { - if (Body == IntPtr.Zero) - enableBody(); - - //Prim auto disable after 20 frames, - //if you move it, re-enable the prim manually. - if (_parent != null) + if (!newLock.ApproxEquals(Vector3.One, 0f)) + { + createAMotor(newLock); + } + else { - if (m_linkJoint != IntPtr.Zero) + if (Amotor != IntPtr.Zero) { - d.JointDestroy(m_linkJoint); - m_linkJoint = IntPtr.Zero; + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; } } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = newLock; + } - if (Body != IntPtr.Zero) + private void changeLink(OdePrim NewParent) + { + if (_parent == null && NewParent != null) + { + NewParent.ParentPrim(this); + } + else if (_parent != null) + { + if (_parent is OdePrim) + { + if (NewParent != _parent) { - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + (_parent as OdePrim).ChildDelink(this, false); // for now... + childPrim = false; - if (_parent != null) - { - OdePrim odParent = (OdePrim)_parent; - if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) - { -// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? -Console.WriteLine(" JointCreateFixed"); - m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); - d.JointAttach(m_linkJoint, Body, odParent.Body); - d.JointSetFixed(m_linkJoint); - } - } - d.BodyEnable(Body); - if (m_vehicle.Type != Vehicle.TYPE_NONE) + if (NewParent != null) { - m_vehicle.Enable(Body, _parent_scene); + NewParent.ParentPrim(this); } } - else - { - m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name); - } } - //else - // { - //m_log.Debug("[BUG]: race!"); - //} } + _parent = NewParent; + } - // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); - // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); -// _parent_scene.waitForSpaceUnlock(m_targetSpace); - - IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); - m_targetSpace = tempspace; -// _parent_scene.waitForSpaceUnlock(m_targetSpace); + private void Stop() + { + if (!childPrim) + { + m_force = Vector3.Zero; + m_forceacc = Vector3.Zero; + m_angularForceacc = Vector3.Zero; + _torque = Vector3.Zero; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + _zeroFlag = false; + base.RequestPhysicsterseUpdate(); + } -// _parent_scene.waitForSpaceUnlock(m_targetSpace); - d.SpaceAdd(m_targetSpace, prim_geom); + if (Body != IntPtr.Zero) + { + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetTorque(Body, 0f, 0f, 0f); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetAngularVel(Body, 0f, 0f, 0f); + } + } - changeSelectedStatus(); + private void changePhantomStatus(bool newval) + { + m_isphantom = newval; - resetCollisionAccounting(); - m_taintposition = _position; + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); } - internal void Move(float timestep) +/* not in use + internal void ChildSelectedChange(bool childSelect) { - float fx = 0; - float fy = 0; - float fz = 0; + if(childPrim) + return; + + if (childSelect == m_isSelected) + return; + + if (childSelect) + { + DoSelectedStatus(true); + } - if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. + else { - if (m_vehicle.Type != Vehicle.TYPE_NONE) + foreach (OdePrim prm in childrenPrim) { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(timestep, _parent_scene); + if (prm.m_isSelected) + return; } - else - { -//Console.WriteLine("Move " + Name); - if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - // NON-'VEHICLES' are dealt with here -// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) -// { -// d.Vector3 avel2 = d.BodyGetAngularVel(Body); -// /* -// if (m_angularlock.X == 1) -// avel2.X = 0; -// if (m_angularlock.Y == 1) -// avel2.Y = 0; -// if (m_angularlock.Z == 1) -// avel2.Z = 0; -// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); -// */ -// } - //float PID_P = 900.0f; - - float m_mass = CalculateMass(); - -// fz = 0f; - //m_log.Info(m_collisionFlags.ToString()); - - - //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? - // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up - // gravityz multiplier = 1 - m_buoyancy - fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; - - if (m_usePID) - { -//Console.WriteLine("PID " + Name); - // KF - this is for object move? eg. llSetPos() ? - //if (!d.BodyIsEnabled(Body)) - //d.BodySetForce(Body, 0f, 0f, 0f); - // If we're using the PID controller, then we have no gravity - //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... - fz = 0f; - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - //PidStatus = true; - - // PhysicsVector vec = new PhysicsVector(); - d.Vector3 vel = d.BodyGetLinearVel(Body); + DoSelectedStatus(false); + } + } +*/ + private void changeSelectedStatus(bool newval) + { + if (m_lastdoneSelected == newval) + return; - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); + m_lastdoneSelected = newval; + DoSelectedStatus(newval); + } - // if velocity is zero, use position control; otherwise, velocity control + private void CheckDelaySelect() + { + if (m_delaySelect) + { + DoSelectedStatus(m_isSelected); + } + } - if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; + private void DoSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + if (newval) + { + if (!childPrim && Body != IntPtr.Zero) + d.BodyDisable(Body); - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end if (m_usePID) + if (m_delaySelect || m_isphysical) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = 0; - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - if (m_useHoverPID && !m_usePID) + if (!childPrim) { -//Console.WriteLine("Hover " + Name); - - // If we're using the PID controller, then we have no gravity - fz = (-1 * _parent_scene.gravityz) * m_mass; - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) + foreach (OdePrim prm in childrenPrim) { - PID_G = m_PIDTau + 1; - } + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); + if (prm.prim_geom != null) + { - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) + if (prm.m_NoColide) { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); } else { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); } - break; - - } // end switch (m_PIDHoverType) - - - _target_velocity = - new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); + } + prm.m_delaySelect = false; + } + } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(true); - // if velocity is zero, use position control; otherwise, velocity control - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + if (prim_geom != null) + { + if (m_NoColide) { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, 0); + d.GeomSetCollideBits(collide_geom, 0); + } + } else { - _zeroFlag = false; - - // We're flying and colliding with something - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); + } } } - fx *= m_mass; - fy *= m_mass; - //fz *= m_mass; - - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - //m_taintdisable = true; - //base.RaiseOutOfBounds(Position); - //d.BodySetLinearVel(Body, fx, fy, 0f); - if (!d.BodyIsEnabled(Body)) - { - // A physical body at rest on a surface will auto-disable after a while, - // this appears to re-enable it incase the surface it is upon vanishes, - // and the body should fall again. - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0, 0, 0); - enableBodySoft(); - } - - // 35x10 = 350n times the mass per second applied maximum. - float nmax = 35f * m_mass; - float nmin = -35f * m_mass; - - if (fx > nmax) - fx = nmax; - if (fx < nmin) - fx = nmin; - if (fy > nmax) - fy = nmax; - if (fy < nmin) - fy = nmin; - d.BodyAddForce(Body, fx, fy, fz); -//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } + m_delaySelect = false; } - } - else - { // is not physical, or is not a body or is selected - // _zeroPosition = d.BodyGetPosition(Body); - return; -//Console.WriteLine("Nothing " + Name); - - } - } - - private void rotate() - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - if (Body != IntPtr.Zero) - { - // KF: If this is a root prim do BodySet - d.BodySetQuaternion(Body, ref myrot); - if (IsPhysical) + else if(!m_isphysical) { - if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); + m_delaySelect = true; } } else { - // daughter prim, do Geom set - d.GeomSetQuaternion(prim_geom, ref myrot); - } - - resetCollisionAccounting(); - m_taintrot = _orientation; - } + if (!childPrim) + { + if (Body != IntPtr.Zero && !m_disabled) + d.BodyEnable(Body); + } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(false); - private void resetCollisionAccounting() - { - m_collisionscore = 0; - m_interpenetrationcount = 0; - m_disabled = false; - } + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); - /// - /// Change prim in response to a disable taint. - /// - private void changedisable() - { - m_disabled = true; - if (Body != IntPtr.Zero) - { - d.BodyDisable(Body); - Body = IntPtr.Zero; + m_delaySelect = false; } - m_taintdisable = false; + resetCollisionAccounting(); } - /// - /// Change prim in response to a physics status taint - /// - private void changePhysicsStatus() + private void changePosition(Vector3 newPos) { - if (IsPhysical) + CheckDelaySelect(); + if (m_isphysical) { - if (Body == IntPtr.Zero) + if (childPrim) // inertia is messed, must rebuild { - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + if (m_building) { - changeshape(); + _position = newPos; } - else + + else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) + { + FixInertia(newPos); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (_position != newPos) { - enableBody(); + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); } } else { - if (Body != IntPtr.Zero) + if (prim_geom != IntPtr.Zero) { - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + if (newPos != _position) { - RemoveGeom(); - -//Console.WriteLine("changePhysicsStatus for " + Name); - changeadd(); - } + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; - if (childPrim) - { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this); - } - } - else - { - disableBody(); + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); } } } - - changeSelectedStatus(); - + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; +// changeSelectedStatus(); resetCollisionAccounting(); - m_taintPhysics = IsPhysical; } - /// - /// Change prim in response to a size taint. - /// - private void changesize() + private void changeOrientation(Quaternion newOri) { -#if SPAM - m_log.DebugFormat("[ODE PRIM]: Called changesize"); -#endif - - if (_size.X <= 0) _size.X = 0.01f; - if (_size.Y <= 0) _size.Y = 0.01f; - if (_size.Z <= 0) _size.Z = 0.01f; - - //kill body to rebuild - if (IsPhysical && Body != IntPtr.Zero) + CheckDelaySelect(); + if (m_isphysical) { - if (childPrim) + if (childPrim) // inertia is messed, must rebuild { - if (_parent != null) + if (m_building) { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this); + _orientation = newOri; + } +/* + else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) + { + FixInertia(_position, newOri); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); } +*/ } else { - disableBody(); + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); } } - - if (d.SpaceQuery(m_targetSpace, prim_geom)) + else { -// _parent_scene.waitForSpaceUnlock(m_targetSpace); - d.SpaceRemove(m_targetSpace, prim_geom); + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + } } + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + resetCollisionAccounting(); + } - RemoveGeom(); - - // we don't need to do space calculation because the client sends a position update also. - - IMesh mesh = null; - - // Construction of new prim - if (_parent_scene.needsMeshing(_pbs)) + private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) { - float meshlod = _parent_scene.meshSculptLOD; - - if (IsPhysical) - meshlod = _parent_scene.MeshSculptphysicalLOD; - // Don't need to re-enable body.. it's done in SetMesh - - if (_parent_scene.needsMeshing(_pbs)) + if (childPrim && m_building) // inertia is messed, must rebuild { - mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); - if (mesh == null) - CheckMeshAsset(); + _position = newPos; + _orientation = newOri; + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); } - } - - CreateGeom(m_targetSpace, mesh); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - - //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); - if (IsPhysical && Body == IntPtr.Zero && !childPrim) + else { - // Re creates body on size. - // EnableBody also does setMass() - enableBody(); - d.BodyEnable(Body); - } + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - changeSelectedStatus(); - - if (childPrim) - { - if (_parent is OdePrim) + if (prim_geom != IntPtr.Zero) { - OdePrim parent = (OdePrim)_parent; - parent.ChildSetGeom(this); + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } } } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; resetCollisionAccounting(); - m_taintsize = _size; } - /// - /// Change prim in response to a float on water taint. - /// - /// - private void changefloatonwater() + private void changeDisable(bool disable) { - m_collidesWater = m_taintCollidesWater; - - if (m_collidesWater) + if (disable) { - m_collisionFlags |= CollisionCategories.Water; + if (!m_disabled) + disableBodySoft(); } else { - m_collisionFlags &= ~CollisionCategories.Water; + if (m_disabled) + enableBodySoft(); } - - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - /// - /// Change prim in response to a shape taint. - /// - private void changeshape() + private void changePhysicsStatus(bool NewStatus) { - m_taintshape = false; + CheckDelaySelect(); - // Cleanup of old prim geometry and Bodies - if (IsPhysical && Body != IntPtr.Zero) + m_isphysical = NewStatus; + + if (!childPrim) { - if (childPrim) + if (NewStatus) { - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this); - } + if (Body == IntPtr.Zero) + MakeBody(); } else { - disableBody(); + if (Body != IntPtr.Zero) + { + DestroyBody(); + } + Stop(); } } - RemoveGeom(); + resetCollisionAccounting(); + } - // we don't need to do space calculation because the client sends a position update also. - if (_size.X <= 0) _size.X = 0.01f; - if (_size.Y <= 0) _size.Y = 0.01f; - if (_size.Z <= 0) _size.Z = 0.01f; - // Construction of new prim + private void changeSize(Vector3 newSize) + { + } - IMesh mesh = null; + private void changeShape(PrimitiveBaseShape newShape) + { + } + private void changeAddPhysRep(ODEPhysRepData repData) + { + _size = repData.size; //?? + _pbs = repData.pbs; + m_shapetype = repData.shapetype; - if (_parent_scene.needsMeshing(_pbs)) - { - // Don't need to re-enable body.. it's done in CreateMesh - float meshlod = _parent_scene.meshSculptLOD; + m_mesh = repData.mesh; - if (IsPhysical) - meshlod = _parent_scene.MeshSculptphysicalLOD; + m_assetID = repData.assetID; + m_meshState = repData.meshState; - // createmesh returns null when it doesn't mesh. - mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); - if (mesh == null) - CheckMeshAsset(); + m_hasOBB = repData.hasOBB; + m_OBBOffset = repData.OBBOffset; + m_OBB = repData.OBB; + + primVolume = repData.volume; + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); } - CreateGeom(m_targetSpace, mesh); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - //myrot.W = _orientation.w; - myrot.W = _orientation.W; - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - d.GeomSetQuaternion(prim_geom, ref myrot); + if (!m_isphysical) + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + else + MakeBody(); - //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); - if (IsPhysical && Body == IntPtr.Zero) + if ((m_meshState & MeshState.NeedMask) != 0) { - // Re creates body on size. - // EnableBody also does setMass() - enableBody(); - if (Body != IntPtr.Zero) - { - d.BodyEnable(Body); - } + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_shapetype; + _parent_scene.m_meshWorker.RequestMesh(repData); } + } - changeSelectedStatus(); + private void changePhysRepData(ODEPhysRepData repData) + { + CheckDelaySelect(); + + OdePrim parent = (OdePrim)_parent; - if (childPrim) + bool chp = childPrim; + + if (chp) { - if (_parent is OdePrim) + if (parent != null) { - OdePrim parent = (OdePrim)_parent; - parent.ChildSetGeom(this); + parent.DestroyBody(); } } + else + { + DestroyBody(); + } - resetCollisionAccounting(); -// m_taintshape = false; - } + RemoveGeom(); - /// - /// Change prim in response to an add force taint. - /// - private void changeAddForce() - { - if (!m_isSelected) + _size = repData.size; + _pbs = repData.pbs; + m_shapetype = repData.shapetype; + + m_mesh = repData.mesh; + + m_assetID = repData.assetID; + m_meshState = repData.meshState; + + m_hasOBB = repData.hasOBB; + m_OBBOffset = repData.OBBOffset; + m_OBB = repData.OBB; + + primVolume = repData.volume; + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (m_isphysical) { - lock (m_forcelist) + if (chp) { - //m_log.Info("[PHYSICS]: dequeing forcelist"); - if (IsPhysical) + if (parent != null) { - Vector3 iforce = Vector3.Zero; - int i = 0; - try - { - for (i = 0; i < m_forcelist.Count; i++) - { - - iforce = iforce + (m_forcelist[i] * 100); - } - } - catch (IndexOutOfRangeException) - { - m_forcelist = new List(); - m_collisionscore = 0; - m_interpenetrationcount = 0; - m_taintforce = false; - return; - } - catch (ArgumentOutOfRangeException) - { - m_forcelist = new List(); - m_collisionscore = 0; - m_interpenetrationcount = 0; - m_taintforce = false; - return; - } - d.BodyEnable(Body); - d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); + parent.MakeBody(); } - m_forcelist.Clear(); } + else + MakeBody(); + } + else + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } - m_collisionscore = 0; - m_interpenetrationcount = 0; + resetCollisionAccounting(); + + if ((m_meshState & MeshState.NeedMask) != 0) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_shapetype; + _parent_scene.m_meshWorker.RequestMesh(repData); } + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; - m_taintforce = false; + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); } - /// - /// Change prim in response to a torque taint. - /// - private void changeSetTorque() + private void changeSetTorque(Vector3 newtorque) { if (!m_isSelected) { - if (IsPhysical && Body != IntPtr.Zero) + if (m_isphysical && Body != IntPtr.Zero) { - d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + _torque = newtorque; } + } - m_taintTorque = Vector3.Zero; + private void changeForce(Vector3 force) + { + m_force = force; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); } - /// - /// Change prim in response to an angular force taint. - /// - private void changeAddAngularForce() + private void changeAddForce(Vector3 theforce) { + m_forceacc += theforce; if (!m_isSelected) { - lock (m_angularforcelist) + lock (this) { //m_log.Info("[PHYSICS]: dequeing forcelist"); - if (IsPhysical) + if (m_isphysical && Body != IntPtr.Zero) { - Vector3 iforce = Vector3.Zero; - for (int i = 0; i < m_angularforcelist.Count; i++) - { - iforce = iforce + (m_angularforcelist[i] * 100); - } - d.BodyEnable(Body); - d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); - + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); } - m_angularforcelist.Clear(); } - m_collisionscore = 0; - m_interpenetrationcount = 0; } - - m_taintaddangularforce = false; } - /// - /// Change prim in response to a velocity taint. - /// - private void changevelocity() + // actually angular impulse + private void changeAddAngularImpulse(Vector3 aimpulse) { + m_angularForceacc += aimpulse * m_invTimeStep; if (!m_isSelected) { - // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar - // walking through a default rez size prim if it keeps kicking it around - justincc. - Thread.Sleep(20); - - if (IsPhysical) + lock (this) { - if (Body != IntPtr.Zero) + if (m_isphysical && Body != IntPtr.Zero) { - d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); } } - - //resetCollisionAccounting(); + m_collisionscore = 0; } - - m_taintVelocity = Vector3.Zero; } - internal void setPrimForRemoval() + private void changevelocity(Vector3 newVel) { - m_taintremove = true; - } + float len = newVel.LengthSquared(); + if (len > 100000.0f) // limit to 100m/s + { + len = 100.0f / (float)Math.Sqrt(len); + newVel *= len; + } - public override bool Flying - { - // no flying prims for you - get { return false; } - set { } - } + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); - public override bool IsColliding - { - get { return iscolliding; } - set { iscolliding = value; } + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; } - public override bool CollidingGround + private void changeangvelocity(Vector3 newAngVel) { - get { return false; } - set { return; } + float len = newAngVel.LengthSquared(); + if (len > 144.0f) // limit to 12rad/s + { + len = 12.0f / (float)Math.Sqrt(len); + newAngVel *= len; + } + + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + + d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); + } + //resetCollisionAccounting(); + } + m_rotationalVelocity = newAngVel; } - public override bool CollidingObj + private void changeVolumedetetion(bool newVolDtc) { - get { return false; } - set { return; } + m_isVolumeDetect = newVolDtc; + m_fakeisVolumeDetect = newVolDtc; + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); } - public override bool ThrottleUpdates + protected void changeBuilding(bool newbuilding) { - get { return m_throttleUpdates; } - set { m_throttleUpdates = value; } + // Check if we need to do anything + if (newbuilding == m_building) + return; + + if ((bool)newbuilding) + { + m_building = true; + if (!childPrim) + DestroyBody(); + } + else + { + m_building = false; + CheckDelaySelect(); + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } } - public override bool Stopped + public void changeSetVehicle(VehicleData vdata) { - get { return _zeroFlag; } + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + m_vehicle.DoSetVehicle(vdata); } - public override Vector3 Position + private void changeVehicleType(int value) { - get { return _position; } + if (value == (int)Vehicle.TYPE_NONE) + { + if (m_vehicle != null) + m_vehicle = null; + } + else + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); - set { _position = value; - //m_log.Info("[PHYSICS]: " + _position.ToString()); + m_vehicle.ProcessTypeChange((Vehicle)value); } } - public override Vector3 Size + private void changeVehicleFloatParam(strVehicleFloatParam fp) { - get { return _size; } - set - { - if (value.IsFinite()) - { - _size = value; -// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); - } - } + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); } - public override float Mass + private void changeVehicleVectorParam(strVehicleVectorParam vp) { - get { return CalculateMass(); } + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); } - public override Vector3 Force + private void changeVehicleRotationParam(strVehicleQuatParam qp) { - //get { return Vector3.Zero; } - get { return m_force; } - set - { - if (value.IsFinite()) - { - m_force = value; - } - else - { - m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); - } - } + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); } - public override int VehicleType + private void changeVehicleFlags(strVehicleBoolParam bp) { - get { return (int)m_vehicle.Type; } - set { m_vehicle.ProcessTypeChange((Vehicle)value); } + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); } - public override void VehicleFloatParam(int param, float value) + private void changeBuoyancy(float b) { - m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value); + m_buoyancy = b; } - public override void VehicleVectorParam(int param, Vector3 value) + private void changePIDTarget(Vector3 trg) { - m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value); + m_PIDTarget = trg; } - public override void VehicleRotationParam(int param, Quaternion rotation) + private void changePIDTau(float tau) { - m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); + m_PIDTau = tau; } - public override void VehicleFlags(int param, bool remove) + private void changePIDActive(bool val) { - m_vehicle.ProcessVehicleFlags(param, remove); + m_usePID = val; } - public override void SetVolumeDetect(int param) + private void changePIDHoverHeight(float val) { - // We have to lock the scene here so that an entire simulate loop either uses volume detect for all - // possible collisions with this prim or for none of them. - lock (_parent_scene.OdeLock) - { - m_isVolumeDetect = (param != 0); - } + m_PIDHoverHeight = val; + if (val == 0) + m_useHoverPID = false; } - public override Vector3 CenterOfMass + private void changePIDHoverType(PIDHoverType type) { - get { return Vector3.Zero; } + m_PIDHoverType = type; } - public override Vector3 GeometricCenter + private void changePIDHoverTau(float tau) { - get { return Vector3.Zero; } + m_PIDHoverTau = tau; } - public override PrimitiveBaseShape Shape + private void changePIDHoverActive(bool active) { - set - { - _pbs = value; - m_assetFailed = false; - m_taintshape = true; - } + m_useHoverPID = active; } - public override Vector3 Velocity - { - get - { - // Average previous velocity with the new one so - // client object interpolation works a 'little' better - if (_zeroFlag) - return Vector3.Zero; + #endregion - Vector3 returnVelocity = Vector3.Zero; - returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2' - returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f; - returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f; - return returnVelocity; - } - set + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && !m_building && !m_outbounds) { - if (value.IsFinite()) + if (!d.BodyIsEnabled(Body)) { - _velocity = value; + // let vehicles sleep + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + return; - m_taintVelocity = value; - _parent_scene.AddPhysicsActorTaint(this); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); - } + if (++bodydisablecontrol < 20) + return; - } - } + + d.BodyEnable(Body); + } - public override Vector3 Torque - { - get - { - if (!IsPhysical || Body == IntPtr.Zero) - return Vector3.Zero; + bodydisablecontrol = 0; - return _torque; - } + d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - set - { - if (value.IsFinite()) + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) { - m_taintTorque = value; - _parent_scene.AddPhysicsActorTaint(this); + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + return; } - else + + float fx = 0; + float fy = 0; + float fz = 0; + + float m_mass = _mass; + + if (m_usePID && m_PIDTau > 0) { - m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); - } - } - } + // for now position error + _target_velocity = + new Vector3( + (m_PIDTarget.X - lpos.X), + (m_PIDTarget.Y - lpos.Y), + (m_PIDTarget.Z - lpos.Z) + ); - public override float CollisionScore - { - get { return m_collisionscore; } - set { m_collisionscore = value; } - } + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) + { + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + return; + } + else + { + _zeroFlag = false; - public override bool Kinematic - { - get { return false; } - set { } - } + float tmp = 1 / m_PIDTau; + _target_velocity *= tmp; - public override Quaternion Orientation - { - get { return _orientation; } - set - { - if (QuaternionIsFinite(value)) - _orientation = value; - else - m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); - } - } + // apply limits + tmp = _target_velocity.Length(); + if (tmp > 50.0f) + { + tmp = 50 / tmp; + _target_velocity *= tmp; + } + else if (tmp < 0.05f) + { + tmp = 0.05f / tmp; + _target_velocity *= tmp; + } - private static bool QuaternionIsFinite(Quaternion q) - { - if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) - return false; - if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) - return false; - if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) - return false; - if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) - return false; - return true; - } + d.Vector3 vel = d.BodyGetLinearVel(Body); + fx = (_target_velocity.X - vel.X) * m_invTimeStep; + fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; + fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; +// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + } // end if (m_usePID) - public override Vector3 Acceleration - { - get { return _acceleration; } - set { _acceleration = value; } - } + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) + { - public override void AddForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - lock (m_forcelist) - m_forcelist.Add(force); + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType - m_taintforce = true; - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); - } - //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); - } + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); - public override void AddAngularForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - m_angularforcelist.Add(force); - m_taintaddangularforce = true; - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); - } - } + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; - public override Vector3 RotationalVelocity - { - get - { - Vector3 pv = Vector3.Zero; - if (_zeroFlag) - return pv; - m_lastUpdateSent = false; + case PIDHoverType.GroundAndWater: + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + else + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + break; + } // end switch (m_PIDHoverType) - if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) - return pv; + // don't go underground unless volumedetector - return m_rotationalVelocity; - } - set - { - if (value.IsFinite()) - { - m_rotationalVelocity = value; + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + { + d.Vector3 vel = d.BodyGetLinearVel(Body); + + fz = (m_targetHoverHeight - lpos.Z); + + // if error is zero, use position control; otherwise, velocity control + if (Math.Abs(fz) < 0.01f) + { + d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + } + else + { + _zeroFlag = false; + fz /= m_PIDHoverTau; + + float tmp = Math.Abs(fz); + if (tmp > 50) + fz = 50 * Math.Sign(fz); + else if (tmp < 0.1) + fz = 0.1f * Math.Sign(fz); + + fz = ((fz - vel.Z) * m_invTimeStep); + } + } } else { - m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; } - } - } - public override void CrossingFailure() - { - m_crossingfailures++; - if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - base.RaiseOutOfBounds(_position); - return; - } - else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name); - } - } + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; - public override float Buoyancy - { - get { return m_buoyancy; } - set { m_buoyancy = value; } - } + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; - public override void link(PhysicsActor obj) - { - m_taintparent = obj; - } + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; - public override void delink() - { - m_taintparent = null; - } + m_forceacc = Vector3.Zero; - public override void LockAngularMotion(Vector3 axis) - { - // reverse the zero/non zero values for ODE. - if (axis.IsFinite()) - { - axis.X = (axis.X > 0) ? 1f : 0f; - axis.Y = (axis.Y > 0) ? 1f : 0f; - axis.Z = (axis.Z > 0) ? 1f : 0f; - m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - m_taintAngularLock = axis; + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + + Vector3 trq; + + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } } else - { - m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; + //Console.WriteLine("Nothing " + Name); + } } - internal void UpdatePositionAndVelocity() + public void UpdatePositionAndVelocity() { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null) + if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) { - Vector3 pv = Vector3.Zero; - bool lastZeroFlag = _zeroFlag; - float m_minvelocity = 0; - if (Body != (IntPtr)0) // FIXME -> or if it is a joint - { - d.Vector3 vec = d.BodyGetPosition(Body); - d.Quaternion ori = d.BodyGetQuaternion(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 rotvel = d.BodyGetAngularVel(Body); - d.Vector3 torque = d.BodyGetTorque(Body); - _torque = new Vector3(torque.X, torque.Y, torque.Z); - Vector3 l_position = Vector3.Zero; - Quaternion l_orientation = Quaternion.Identity; - - // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) - //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - - m_lastposition = _position; - m_lastorientation = _orientation; - - l_position.X = vec.X; - l_position.Y = vec.Y; - l_position.Z = vec.Z; - l_orientation.X = ori.X; - l_orientation.Y = ori.Y; - l_orientation.Z = ori.Z; - l_orientation.W = ori.W; - - if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) - { - //base.RaiseOutOfBounds(l_position); + if (d.BodyIsEnabled(Body) || !_zeroFlag) + { + bool lastZeroFlag = _zeroFlag; - if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - _position = l_position; - //_parent_scene.remActivePrim(this); - if (_parent == null) - base.RequestPhysicsterseUpdate(); - return; - } - else - { - if (_parent == null) - base.RaiseOutOfBounds(l_position); - return; - } - } + d.Vector3 lpos = d.GeomGetPosition(prim_geom); - if (l_position.Z < 0) + // check outside region + if (lpos.Z < -100 || lpos.Z > 100000f) { - // This is so prim that get lost underground don't fall forever and suck up - // - // Sim resources and memory. - // Disables the prim's movement physics.... - // It's a hack and will generate a console message if it fails. - - //IsPhysical = false; - if (_parent == null) - base.RaiseOutOfBounds(_position); + m_outbounds = true; + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); _acceleration.X = 0; _acceleration.Y = 0; _acceleration.Z = 0; @@ -2698,589 +3401,437 @@ Console.WriteLine(" JointCreateFixed"); m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; - if (_parent == null) - base.RequestPhysicsterseUpdate(); + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; - m_throttleUpdates = false; - throttleCounter = 0; + base.RequestPhysicsterseUpdate(); + +// throttleCounter = 0; _zeroFlag = true; - //outofBounds = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if (m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + UnSubscribeEvents(); + + base.RequestPhysicsterseUpdate(); + return; } - //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); -//Console.WriteLine("Adiff " + Name + " = " + Adiff); - if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) - && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) - && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) -// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) - && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + + // decide if moving + // use positions since this are integrated quantities + // tolerance values depende a lot on simulation noise... + // use simple math.abs since we dont need to be exact + + if ( + (Math.Abs(_position.X - lpos.X) < 0.001f) + && (Math.Abs(_position.Y - lpos.Y) < 0.001f) + && (Math.Abs(_position.Z - lpos.Z) < 0.001f) + && (Math.Abs(_orientation.X - ori.X) < 0.0001f) + && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f) + && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W + ) { _zeroFlag = true; -//Console.WriteLine("ZFT 2"); - m_throttleUpdates = false; } else - { - //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); _zeroFlag = false; - m_lastUpdateSent = false; - //m_throttleUpdates = false; - } - if (_zeroFlag) + // update velocities and aceleration + if (!(_zeroFlag && lastZeroFlag)) { - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; + d.Vector3 vel = d.BodyGetLinearVel(Body); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; + _acceleration = _velocity; - //_orientation.w = 0f; - //_orientation.X = 0f; - //_orientation.Y = 0f; - //_orientation.Z = 0f; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - if (!m_lastUpdateSent) + if ((Math.Abs(vel.X) < 0.001f) && + (Math.Abs(vel.Y) < 0.001f) && + (Math.Abs(vel.Z) < 0.001f)) { - m_throttleUpdates = false; - throttleCounter = 0; - m_rotationalVelocity = pv; - - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } - - m_lastUpdateSent = true; + _velocity = Vector3.Zero; + float t = -m_invTimeStep; + _acceleration = _acceleration * t; } - } - else - { - if (lastZeroFlag != _zeroFlag) + else { - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + _acceleration = (_velocity - _acceleration) * m_invTimeStep; } - m_lastVelocity = _velocity; - - _position = l_position; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - - _acceleration = ((_velocity - m_lastVelocity) / 0.1f); - _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); - //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); - - // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... - // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. - // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles - // adding these logical exclusion situations to maintain this where I think it was intended to be. - if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) - { - m_minvelocity = 0.5f; - } - else + if ((Math.Abs(_acceleration.X) < 0.01f) && + (Math.Abs(_acceleration.Y) < 0.01f) && + (Math.Abs(_acceleration.Z) < 0.01f)) { - m_minvelocity = 0.02f; + _acceleration = Vector3.Zero; } - if (_velocity.ApproxEquals(pv, m_minvelocity)) + if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && + (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && + (Math.Abs(_orientation.Z - ori.Z) < 0.0001) + ) { - m_rotationalVelocity = pv; + m_rotationalVelocity = Vector3.Zero; } else { - m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); + vel = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = vel.X; + m_rotationalVelocity.Y = vel.Y; + m_rotationalVelocity.Z = vel.Z; } + } - //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + if (_zeroFlag) + { + if (lastZeroFlag) { - if (_parent == null) - { - base.RequestPhysicsterseUpdate(); - } + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; } - else + + if (!m_lastUpdateSent) { - throttleCounter++; + base.RequestPhysicsterseUpdate(); + if (lastZeroFlag) + m_lastUpdateSent = true; } + return; } - m_lastposition = l_position; - } - else - { - // Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + base.RequestPhysicsterseUpdate(); + m_lastUpdateSent = false; } } } - public override bool FloatOnWater + internal static bool QuaternionIsFinite(Quaternion q) { - set { - m_taintCollidesWater = value; - _parent_scene.AddPhysicsActorTaint(this); - } + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; } - public override void SetMomentum(Vector3 momentum) + internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) { - } - - public override Vector3 PIDTarget - { - set - { - if (value.IsFinite()) - { - m_PIDTarget = value; - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); - } - } - public override bool PIDActive { set { m_usePID = value; } } - public override float PIDTau { set { m_PIDTau = value; } } - - public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } - public override bool PIDHoverActive { set { m_useHoverPID = value; } } - public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } - public override float PIDHoverTau { set { m_PIDHoverTau = value; } } - - public override Quaternion APIDTarget{ set { return; } } + // assumes object center of mass is zero + float smass = part.mass; + theobj.mass -= smass; - public override bool APIDActive{ set { return; } } + smass *= 1.0f / (theobj.mass); ; - public override float APIDStrength{ set { return; } } + theobj.c.X -= part.c.X * smass; + theobj.c.Y -= part.c.Y * smass; + theobj.c.Z -= part.c.Z * smass; - public override float APIDDamping{ set { return; } } + theobj.I.M00 -= part.I.M00; + theobj.I.M01 -= part.I.M01; + theobj.I.M02 -= part.I.M02; + theobj.I.M10 -= part.I.M10; + theobj.I.M11 -= part.I.M11; + theobj.I.M12 -= part.I.M12; + theobj.I.M20 -= part.I.M20; + theobj.I.M21 -= part.I.M21; + theobj.I.M22 -= part.I.M22; + } - private void createAMotor(Vector3 axis) + private void donullchange() { - if (Body == IntPtr.Zero) - return; + } - if (Amotor != IntPtr.Zero) + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; + return false; } - float axisnum = 3; + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; - axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); + case changes.AddPhysRep: + changeAddPhysRep((ODEPhysRepData)arg); + break; - // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); + case changes.Remove: + //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... + //When we return true, it destroys all of the prims in the linkset anyway + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildRemove(this, false); + } + else + ChildRemove(this, false); - - // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. - d.Mass objMass; - d.MassSetZero(out objMass); - DMassCopy(ref pMass, ref objMass); + m_vehicle = null; + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + UnSubscribeEvents(); + return true; - //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); + case changes.Link: + OdePrim tmp = (OdePrim)arg; + changeLink(tmp); + break; - Matrix4 dMassMat = FromDMass(objMass); + case changes.DeLink: + changeLink(null); + break; - Matrix4 mathmat = Inverse(dMassMat); + case changes.Position: + changePosition((Vector3)arg); + break; - /* - //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]); + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; - mathmat = Inverse(mathmat); + case changes.PosOffset: + donullchange(); + break; + case changes.OriOffset: + donullchange(); + break; - objMass = FromMatrix4(mathmat, ref objMass); - //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); + case changes.Velocity: + changevelocity((Vector3)arg); + break; - mathmat = Inverse(mathmat); - */ - if (axis.X == 0) - { - mathmat.M33 = 50.0000001f; - //objMass.I.M22 = 0; - } - if (axis.Y == 0) - { - mathmat.M22 = 50.0000001f; - //objMass.I.M11 = 0; - } - if (axis.Z == 0) - { - mathmat.M11 = 50.0000001f; - //objMass.I.M00 = 0; - } - - +// case changes.Acceleration: +// changeacceleration((Vector3)arg); +// break; - mathmat = Inverse(mathmat); - objMass = FromMatrix4(mathmat, ref objMass); - //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); - - //return; - if (d.MassCheck(ref objMass)) - { - d.BodySetMass(Body, ref objMass); - } - else - { - //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); - } + case changes.AngVelocity: + changeangvelocity((Vector3)arg); + break; - if (axisnum <= 0) - return; - // int dAMotorEuler = 1; + case changes.Force: + changeForce((Vector3)arg); + break; - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - d.JointSetAMotorMode(Amotor, 0); + case changes.Torque: + changeSetTorque((Vector3)arg); + break; - d.JointSetAMotorNumAxes(Amotor,(int)axisnum); - int i = 0; + case changes.AddForce: + changeAddForce((Vector3)arg); + break; - if (axis.X == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); - i++; - } + case changes.AddAngForce: + changeAddAngularImpulse((Vector3)arg); + break; - if (axis.Y == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); - i++; - } + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; - if (axis.Z == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); - i++; - } + case changes.Size: + changeSize((Vector3)arg); + break; - for (int j = 0; j < (int)axisnum; j++) - { - //d.JointSetAMotorAngle(Amotor, j, 0); - } + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; - //d.JointSetAMotorAngle(Amotor, 1, 0); - //d.JointSetAMotorAngle(Amotor, 2, 0); + case changes.PhysRepData: + changePhysRepData((ODEPhysRepData) arg); + break; - // These lowstops and high stops are effectively (no wiggle room) - d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); - //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f); - d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// - } + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; - private Matrix4 FromDMass(d.Mass pMass) - { - Matrix4 obj; - obj.M11 = pMass.I.M00; - obj.M12 = pMass.I.M01; - obj.M13 = pMass.I.M02; - obj.M14 = 0; - obj.M21 = pMass.I.M10; - obj.M22 = pMass.I.M11; - obj.M23 = pMass.I.M12; - obj.M24 = 0; - obj.M31 = pMass.I.M20; - obj.M32 = pMass.I.M21; - obj.M33 = pMass.I.M22; - obj.M34 = 0; - obj.M41 = 0; - obj.M42 = 0; - obj.M43 = 0; - obj.M44 = 1; - return obj; - } + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; - private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) - { - obj.I.M00 = pMat[0, 0]; - obj.I.M01 = pMat[0, 1]; - obj.I.M02 = pMat[0, 2]; - obj.I.M10 = pMat[1, 0]; - obj.I.M11 = pMat[1, 1]; - obj.I.M12 = pMat[1, 2]; - obj.I.M20 = pMat[2, 0]; - obj.I.M21 = pMat[2, 1]; - obj.I.M22 = pMat[2, 2]; - return obj; - } + case changes.Phantom: + changePhantomStatus((bool)arg); + break; - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - _parent_scene.AddCollisionEventReporting(this); - } + case changes.Physical: + changePhysicsStatus((bool)arg); + break; - public override void UnSubscribeEvents() - { - _parent_scene.RemoveCollisionEventReporting(this); - m_eventsubscription = 0; - } + case changes.Selected: + changeSelectedStatus((bool)arg); + break; - public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) - { - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - } + case changes.disabled: + changeDisable((bool)arg); + break; - public void SendCollisions() - { - if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0) - { - base.SendCollisionUpdate(CollisionEventsThisFrame); + case changes.building: + changeBuilding((bool)arg); + break; - if (CollisionEventsThisFrame.Count > 0) - { - m_collisionsOnPreviousFrame = true; - CollisionEventsThisFrame.Clear(); - } - else - { - m_collisionsOnPreviousFrame = false; - } - } - } + case changes.VehicleType: + changeVehicleType((int)arg); + break; - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } + case changes.VehicleFlags: + changeVehicleFlags((strVehicleBoolParam) arg); + break; - public static Matrix4 Inverse(Matrix4 pMat) - { - if (determinant3x3(pMat) == 0) - { - return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible - } + case changes.VehicleFloatParam: + changeVehicleFloatParam((strVehicleFloatParam) arg); + break; - return (Adjoint(pMat) / determinant3x3(pMat)); - } + case changes.VehicleVectorParam: + changeVehicleVectorParam((strVehicleVectorParam) arg); + break; - public static Matrix4 Adjoint(Matrix4 pMat) - { - Matrix4 adjointMatrix = new Matrix4(); - for (int i=0; i<4; i++) - { - for (int j=0; j<4; j++) - { - Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); - } - } + case changes.VehicleRotationParam: + changeVehicleRotationParam((strVehicleQuatParam) arg); + break; - adjointMatrix = Transpose(adjointMatrix); - return adjointMatrix; - } + case changes.SetVehicle: + changeSetVehicle((VehicleData) arg); + break; - public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) - { - Matrix4 minor = new Matrix4(); - int m = 0, n = 0; - for (int i = 0; i < 4; i++) - { - if (i == iRow) - continue; - n = 0; - for (int j = 0; j < 4; j++) - { - if (j == iCol) - continue; - Matrix4SetValue(ref minor, m,n, matrix[i, j]); - n++; - } - m++; - } + case changes.Buoyancy: + changeBuoyancy((float)arg); + break; - return minor; - } + case changes.PIDTarget: + changePIDTarget((Vector3)arg); + break; - public static Matrix4 Transpose(Matrix4 pMat) - { - Matrix4 transposeMatrix = new Matrix4(); - for (int i = 0; i < 4; i++) - for (int j = 0; j < 4; j++) - Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); - return transposeMatrix; - } + case changes.PIDTau: + changePIDTau((float)arg); + break; - public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) - { - switch (r) - { - case 0: - switch (c) - { - case 0: - pMat.M11 = val; - break; - case 1: - pMat.M12 = val; - break; - case 2: - pMat.M13 = val; - break; - case 3: - pMat.M14 = val; - break; - } + case changes.PIDActive: + changePIDActive((bool)arg); + break; + case changes.PIDHoverHeight: + changePIDHoverHeight((float)arg); break; - case 1: - switch (c) - { - case 0: - pMat.M21 = val; - break; - case 1: - pMat.M22 = val; - break; - case 2: - pMat.M23 = val; - break; - case 3: - pMat.M24 = val; - break; - } + case changes.PIDHoverType: + changePIDHoverType((PIDHoverType)arg); break; - case 2: - switch (c) - { - case 0: - pMat.M31 = val; - break; - case 1: - pMat.M32 = val; - break; - case 2: - pMat.M33 = val; - break; - case 3: - pMat.M34 = val; - break; - } + case changes.PIDHoverTau: + changePIDHoverTau((float)arg); break; - case 3: - switch (c) - { - case 0: - pMat.M41 = val; - break; - case 1: - pMat.M42 = val; - break; - case 2: - pMat.M43 = val; - break; - case 3: - pMat.M44 = val; - break; - } + case changes.PIDHoverActive: + changePIDHoverActive((bool)arg); + break; + + case changes.Null: + donullchange(); + break; + + + + default: + donullchange(); break; } + return false; } - private static float determinant3x3(Matrix4 pMat) + public void AddChange(changes what, object arg) { - float det = 0; - float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; - float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; - float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; - float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; - float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; - float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; - - det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); - return det; - } - - private static void DMassCopy(ref d.Mass src, ref d.Mass dst) - { - dst.c.W = src.c.W; - dst.c.X = src.c.X; - dst.c.Y = src.c.Y; - dst.c.Z = src.c.Z; - dst.mass = src.mass; - dst.I.M00 = src.I.M00; - dst.I.M01 = src.I.M01; - dst.I.M02 = src.I.M02; - dst.I.M10 = src.I.M10; - dst.I.M11 = src.I.M11; - dst.I.M12 = src.I.M12; - dst.I.M20 = src.I.M20; - dst.I.M21 = src.I.M21; - dst.I.M22 = src.I.M22; + _parent_scene.AddChange((PhysicsActor) this, what, arg); } - public override void SetMaterial(int pMaterial) + + private struct strVehicleBoolParam { - m_material = pMaterial; + public int param; + public bool value; } - private void CheckMeshAsset() + private struct strVehicleFloatParam { - if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero) - { - m_assetFailed = true; - Util.FireAndForget(delegate - { - RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod; - if (assetProvider != null) - assetProvider(_pbs.SculptTexture, MeshAssetReveived); - }); - } + public int param; + public float value; } - void MeshAssetReveived(AssetBase asset) + private struct strVehicleQuatParam { - if (asset.Data != null && asset.Data.Length > 0) - { - if (!_pbs.SculptEntry) - return; - if (_pbs.SculptTexture.ToString() != asset.ID) - return; + public int param; + public Quaternion value; + } - _pbs.SculptData = new byte[asset.Data.Length]; - asset.Data.CopyTo(_pbs.SculptData, 0); - m_assetFailed = false; - m_taintshape = true; - _parent_scene.AddPhysicsActorTaint(this); - } - } + private struct strVehicleVectorParam + { + public int param; + public Vector3 value; + } } -} \ No newline at end of file +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 7a50c4c..03048a4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -25,26 +25,21 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -//#define USE_DRAWSTUFF //#define SPAM using System; using System.Collections.Generic; -using System.Diagnostics; -using System.IO; -using System.Linq; using System.Reflection; using System.Runtime.InteropServices; using System.Threading; +using System.IO; +using System.Diagnostics; using log4net; using Nini.Config; -using Ode.NET; -using OpenMetaverse; -#if USE_DRAWSTUFF -using Drawstuff.NET; -#endif +using OdeAPI; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; +using OpenMetaverse; namespace OpenSim.Region.Physics.OdePlugin { @@ -55,29 +50,42 @@ namespace OpenSim.Region.Physics.OdePlugin End = 2 } -// public struct sCollisionData -// { -// public uint ColliderLocalId; -// public uint CollidedWithLocalId; -// public int NumberOfCollisions; -// public int CollisionType; -// public int StatusIndicator; -// public int lastframe; -// } + public struct sCollisionData + { + public uint ColliderLocalId; + public uint CollidedWithLocalId; + public int NumberOfCollisions; + public int CollisionType; + public int StatusIndicator; + public int lastframe; + } + + + // colision flags of things others can colide with + // rays, sensors, probes removed since can't be colided with + // The top space where things are placed provided further selection + // ie physical are in active space nonphysical in static + // this should be exclusive as possible [Flags] - public enum CollisionCategories : int + public enum CollisionCategories : uint { Disabled = 0, - Geom = 0x00000001, - Body = 0x00000002, - Space = 0x00000004, - Character = 0x00000008, - Land = 0x00000010, - Water = 0x00000020, - Wind = 0x00000040, - Sensor = 0x00000080, - Selected = 0x00000100 + //by 'things' types + Space = 0x01, + Geom = 0x02, // aka prim/part + Character = 0x04, + Land = 0x08, + Water = 0x010, + + // by state + Phantom = 0x01000, + VolumeDtc = 0x02000, + Selected = 0x04000, + NoShape = 0x08000, + + + All = 0xffffffff } /// @@ -98,400 +106,213 @@ namespace OpenSim.Region.Physics.OdePlugin /// Plastic = 5, /// - Rubber = 6 + Rubber = 6, + + light = 7 // compatibility with old viewers + } + + public enum changes : int + { + Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) + Remove, + Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root + // or removes from a object if arg is null + DeLink, + Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child + Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child + PosOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + OriOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + Velocity, + AngVelocity, + Acceleration, + Force, + Torque, + Momentum, + + AddForce, + AddAngForce, + AngLock, + + Buoyancy, + + PIDTarget, + PIDTau, + PIDActive, + + PIDHoverHeight, + PIDHoverType, + PIDHoverTau, + PIDHoverActive, + + Size, + Shape, + PhysRepData, + AddPhysRep, + + CollidesWater, + VolumeDtc, + + Physical, + Phantom, + Selected, + disabled, + building, + + VehicleType, + VehicleFloatParam, + VehicleVectorParam, + VehicleRotationParam, + VehicleFlags, + SetVehicle, + + Null //keep this last used do dim the methods array. does nothing but pulsing the prim } + public struct ODEchangeitem + { + public PhysicsActor actor; + public OdeCharacter character; + public changes what; + public Object arg; + } + public class OdeScene : PhysicsScene { private readonly ILog m_log; // private Dictionary m_storedCollisions = new Dictionary(); - /// - /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances. - /// - /// - /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a - /// time, even where physics objects are in entirely different ODE worlds. This is because generating contacts - /// uses a static cache at the ODE level. - /// - /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar - /// to - /// - /// mono() [0x489171] - /// mono() [0x4d154f] - /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60] - /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a] - /// - /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option - /// causes OpenSimulator to immediately crash with a native stack trace similar to - /// - /// mono() [0x489171] - /// mono() [0x4d154f] - /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60] - /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82] - /// - internal static Object UniversalColliderSyncObject = new Object(); - - /// - /// Is stats collecting enabled for this ODE scene? - /// - public bool CollectStats { get; set; } - - /// - /// Statistics for this scene. - /// - private Dictionary m_stats = new Dictionary(); - - /// - /// Stat name for total number of avatars in this ODE scene. - /// - public const string ODETotalAvatarsStatName = "ODETotalAvatars"; - - /// - /// Stat name for total number of prims in this ODE scene. - /// - public const string ODETotalPrimsStatName = "ODETotalPrims"; - - /// - /// Stat name for total number of prims with active physics in this ODE scene. - /// - public const string ODEActivePrimsStatName = "ODEActivePrims"; - - /// - /// Stat name for the total time spent in ODE frame processing. - /// - /// - /// A sanity check for the main scene loop physics time. - /// - public const string ODETotalFrameMsStatName = "ODETotalFrameMS"; - - /// - /// Stat name for time spent processing avatar taints per frame - /// - public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS"; - - /// - /// Stat name for time spent processing prim taints per frame - /// - public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS"; - - /// - /// Stat name for time spent calculating avatar forces per frame. - /// - public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS"; - - /// - /// Stat name for time spent calculating prim forces per frame - /// - public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS"; - - /// - /// Stat name for time spent fulfilling raycasting requests per frame - /// - public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS"; - - /// - /// Stat name for time spent in native code that actually steps through the simulation. - /// - public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS"; - - /// - /// Stat name for the number of milliseconds that ODE spends in native space collision code. - /// - public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS"; - - /// - /// Stat name for milliseconds that ODE spends in native geom collision code. - /// - public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS"; - - /// - /// Time spent in collision processing that is not spent in native space or geom collision code. - /// - public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS"; - - /// - /// Stat name for time spent notifying listeners of collisions - /// - public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS"; - - /// - /// Stat name for milliseconds spent updating avatar position and velocity - /// - public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS"; - - /// - /// Stat name for the milliseconds spent updating prim position and velocity - /// - public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS"; - - /// - /// Stat name for avatar collisions with another entity. - /// - public const string ODEAvatarContactsStatsName = "ODEAvatarContacts"; - - /// - /// Stat name for prim collisions with another entity. - /// - public const string ODEPrimContactsStatName = "ODEPrimContacts"; - - /// - /// Used to hold tick numbers for stat collection purposes. - /// - private int m_nativeCollisionStartTick; - - /// - /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback. - /// - private bool m_inCollisionTiming; - - /// - /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object - /// collisions occured using the _perloopcontact if stats collection is enabled. - /// - private int m_tempAvatarCollisionsThisFrame; + public bool OdeUbitLib = false; +// private int threadid = 0; + private Random fluidRandomizer = new Random(Environment.TickCount); - /// - /// Used in calculating physics frame time dilation - /// - private int tickCountFrameRun; + const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; + const float MaxERP = 0.8f; + const float minERP = 0.1f; + const float comumContactCFM = 0.0001f; + + float frictionMovementMult = 0.8f; - /// - /// Used in calculating physics frame time dilation - /// - private int latertickcount; + float TerrainBounce = 0.1f; + float TerrainFriction = 0.3f; - private Random fluidRandomizer = new Random(Environment.TickCount); + public float AvatarFriction = 0;// 0.9f * 0.5f; private const uint m_regionWidth = Constants.RegionSize; private const uint m_regionHeight = Constants.RegionSize; - private float ODE_STEPSIZE = 0.0178f; - private float metersInSpace = 29.9f; + public float ODE_STEPSIZE = 0.020f; + public float HalfOdeStep = 0.01f; + public int odetimestepMS = 20; // rounded + private float metersInSpace = 25.6f; private float m_timeDilation = 1.0f; + private DateTime m_lastframe; + private DateTime m_lastMeshExpire; + public float gravityx = 0f; public float gravityy = 0f; public float gravityz = -9.8f; - public float AvatarTerminalVelocity { get; set; } - - private float contactsurfacelayer = 0.001f; - - private int worldHashspaceLow = -4; - private int worldHashspaceHigh = 128; - - private int smallHashspaceLow = -4; - private int smallHashspaceHigh = 66; - private float waterlevel = 0f; private int framecount = 0; - //private int m_returncollisions = 10; - private readonly IntPtr contactgroup; + private int m_meshExpireCntr; - internal IntPtr WaterGeom; +// private IntPtr WaterGeom = IntPtr.Zero; +// private IntPtr WaterHeightmapData = IntPtr.Zero; +// private GCHandle WaterMapHandler = new GCHandle(); - private float nmTerrainContactFriction = 255.0f; - private float nmTerrainContactBounce = 0.1f; - private float nmTerrainContactERP = 0.1025f; - - private float mTerrainContactFriction = 75f; - private float mTerrainContactBounce = 0.1f; - private float mTerrainContactERP = 0.05025f; - - private float nmAvatarObjectContactFriction = 250f; - private float nmAvatarObjectContactBounce = 0.1f; - - private float mAvatarObjectContactFriction = 75f; - private float mAvatarObjectContactBounce = 0.1f; - - private float avPIDD = 3200f; - private float avPIDP = 1400f; + public float avPIDD = 2200f; // make it visible + public float avPIDP = 900f; // make it visible private float avCapRadius = 0.37f; - private float avStandupTensor = 2000000f; - - /// - /// true = old compatibility mode with leaning capsule; false = new corrected mode - /// - /// - /// Even when set to false, the capsule still tilts but this is done in a different way. - /// - public bool IsAvCapsuleTilted { get; private set; } - - private float avDensity = 80f; -// private float avHeightFudgeFactor = 0.52f; + private float avDensity = 3f; private float avMovementDivisorWalk = 1.3f; private float avMovementDivisorRun = 0.8f; private float minimumGroundFlightOffset = 3f; public float maximumMassObject = 10000.01f; - public bool meshSculptedPrim = true; - public bool forceSimplePrimMeshing = false; - - public float meshSculptLOD = 32; - public float MeshSculptphysicalLOD = 16; public float geomDefaultDensity = 10.000006836f; public int geomContactPointsStartthrottle = 3; public int geomUpdatesPerThrottledUpdate = 15; - private const int avatarExpectedContacts = 3; public float bodyPIDD = 35f; public float bodyPIDG = 25; - public int geomCrossingFailuresBeforeOutofbounds = 5; - - public float bodyMotorJointMaxforceTensor = 2; +// public int geomCrossingFailuresBeforeOutofbounds = 6; - public int bodyFramesAutoDisable = 20; + public int bodyFramesAutoDisable = 5; - private float[] _watermap; - private bool m_filterCollisions = true; private d.NearCallback nearCallback; - public d.TriCallback triCallback; - public d.TriArrayCallback triArrayCallback; - - /// - /// Avatars in the physics scene. - /// - private readonly HashSet _characters = new HashSet(); - - /// - /// Prims in the physics scene. - /// - private readonly HashSet _prims = new HashSet(); - /// - /// Prims in the physics scene that are subject to physics, not just collisions. - /// - private readonly HashSet _activeprims = new HashSet(); + private HashSet _characters = new HashSet(); + private HashSet _prims = new HashSet(); + private HashSet _activeprims = new HashSet(); + private HashSet _activegroups = new HashSet(); - /// - /// Prims that the simulator has created/deleted/updated and so need updating in ODE. - /// - private readonly HashSet _taintedPrims = new HashSet(); + public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); /// - /// Record a character that has taints to be processed. + /// A list of actors that should receive collision events. /// - private readonly HashSet _taintedActors = new HashSet(); + private List _collisionEventPrim = new List(); + private List _collisionEventPrimRemove = new List(); + + private HashSet _badCharacter = new HashSet(); +// public Dictionary geom_name_map = new Dictionary(); + public Dictionary actor_name_map = new Dictionary(); - /// - /// Keep record of contacts in the physics loop so that we can remove duplicates. - /// - private readonly List _perloopContact = new List(); + private float contactsurfacelayer = 0.002f; - /// - /// A dictionary of actors that should receive collision events. - /// - private readonly Dictionary m_collisionEventActors = new Dictionary(); + private int contactsPerCollision = 80; + internal IntPtr ContactgeomsArray = IntPtr.Zero; + private IntPtr GlobalContactsArray = IntPtr.Zero; - /// - /// A dictionary of collision event changes that are waiting to be processed. - /// - private readonly Dictionary m_collisionEventActorsChanges = new Dictionary(); + const int maxContactsbeforedeath = 4000; + private volatile int m_global_contactcount = 0; - /// - /// Maps a unique geometry id (a memory location) to a physics actor name. - /// - /// - /// Only actors participating in collisions have geometries. This has to be maintained separately from - /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own - /// apart from the singleton PANull - /// - public Dictionary geom_name_map = new Dictionary(); + private IntPtr contactgroup; - /// - /// Maps a unique geometry id (a memory location) to a physics actor. - /// - /// - /// Only actors participating in collisions have geometries. - /// - public Dictionary actor_name_map = new Dictionary(); + public ContactData[] m_materialContactsData = new ContactData[8]; - /// - /// Defects list to remove characters that no longer have finite positions due to some other bug. - /// - /// - /// Used repeatedly in Simulate() but initialized once here. - /// - private readonly List defects = new List(); + private Dictionary RegionTerrain = new Dictionary(); + private Dictionary TerrainHeightFieldHeights = new Dictionary(); + private Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); + + private int m_physicsiterations = 10; + private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag +// private PhysicsActor PANull = new NullPhysicsActor(); + private float step_time = 0.0f; - private bool m_NINJA_physics_joints_enabled = false; - //private Dictionary jointpart_name_map = new Dictionary(); - private readonly Dictionary> joints_connecting_actor = new Dictionary>(); - private d.ContactGeom[] contacts; + public IntPtr world; - /// - /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active - /// - private readonly List requestedJointsToBeCreated = new List(); - /// - /// can lock for longer. accessed only by OdeScene. - /// - private readonly List pendingJoints = new List(); + // split the spaces acording to contents type + // ActiveSpace contains characters and active prims + // StaticSpace contains land and other that is mostly static in enviroment + // this can contain subspaces, like the grid in staticspace + // as now space only contains this 2 top spaces - /// - /// can lock for longer. accessed only by OdeScene. - /// - private readonly List activeJoints = new List(); + public IntPtr TopSpace; // the global space + public IntPtr ActiveSpace; // space for active prims + public IntPtr StaticSpace; // space for the static things around + public IntPtr GroundSpace; // space for ground - /// - /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active - /// - private readonly List requestedJointsToBeDeleted = new List(); - - private Object externalJointRequestsLock = new Object(); - private readonly Dictionary SOPName_to_activeJoint = new Dictionary(); - private readonly Dictionary SOPName_to_pendingJoint = new Dictionary(); - private readonly DoubleDictionary RegionTerrain = new DoubleDictionary(); - private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); - - private d.Contact contact; - private d.Contact TerrainContact; - private d.Contact AvatarMovementprimContact; - private d.Contact AvatarMovementTerrainContact; - private d.Contact WaterContact; - private d.Contact[,] m_materialContacts; - -//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it -//Ckrinke private int m_randomizeWater = 200; - private int m_physicsiterations = 10; - private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag - private readonly PhysicsActor PANull = new NullPhysicsActor(); -// private float step_time = 0.0f; -//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it -//Ckrinke private int ms = 0; - public IntPtr world; - //private bool returncollisions = false; - // private uint obj1LocalID = 0; - private uint obj2LocalID = 0; - //private int ctype = 0; - private OdeCharacter cc1; - private OdePrim cp1; - private OdeCharacter cc2; - private OdePrim cp2; - private int p1ExpectedPoints = 0; - private int p2ExpectedPoints = 0; - //private int cStartStop = 0; - //private string cDictKey = ""; - - public IntPtr space; - - //private IntPtr tmpSpace; - // split static geometry collision handling into spaces of 30 meters - public IntPtr[,] staticPrimspace; + // some speedup variables + private int spaceGridMaxX; + private int spaceGridMaxY; + private float spacesPerMeter; - /// - /// Used to lock the entire physics scene. Locked during the main part of Simulate() - /// - internal Object OdeLock = new Object(); + // split static geometry collision into a grid as before + private IntPtr[,] staticPrimspace; + private IntPtr[] staticPrimspaceOffRegion; - private bool _worldInitialized = false; + public Object OdeLock; + public static Object SimulationLock; public IMesher mesher; @@ -501,461 +322,340 @@ namespace OpenSim.Region.Physics.OdePlugin public int physics_logging_interval = 0; public bool physics_logging_append_existing_logfile = false; - - public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f); - public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f); - - // TODO: unused: private uint heightmapWidth = m_regionWidth + 1; - // TODO: unused: private uint heightmapHeight = m_regionHeight + 1; - // TODO: unused: private uint heightmapWidthSamples; - // TODO: unused: private uint heightmapHeightSamples; - - private volatile int m_global_contactcount = 0; - private Vector3 m_worldOffset = Vector3.Zero; public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); private PhysicsScene m_parentScene = null; private ODERayCastRequestManager m_rayCastManager; + public ODEMeshWorker m_meshWorker; +/* maybe needed if ode uses tls + private void checkThread() + { + + int th = Thread.CurrentThread.ManagedThreadId; + if(th != threadid) + { + threadid = th; + d.AllocateODEDataForThread(~0U); + } + } + */ /// /// Initiailizes the scene /// Sets many properties that ODE requires to be stable /// These settings need to be tweaked 'exactly' right or weird stuff happens. /// - /// Name of the scene. Useful in debug messages. - public OdeScene(string name) - { - m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + name); + public OdeScene(string sceneIdentifier) + { + m_log + = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); - Name = name; +// checkThread(); + Name = sceneIdentifier; + + OdeLock = new Object(); + SimulationLock = new Object(); nearCallback = near; - triCallback = TriCallback; - triArrayCallback = TriArrayCallback; + m_rayCastManager = new ODERayCastRequestManager(this); + + + lock (OdeLock) + { + // Create the world and the first space + try + { + world = d.WorldCreate(); + TopSpace = d.HashSpaceCreate(IntPtr.Zero); - // Create the world and the first space - world = d.WorldCreate(); - space = d.HashSpaceCreate(IntPtr.Zero); + // now the major subspaces + ActiveSpace = d.HashSpaceCreate(TopSpace); + StaticSpace = d.HashSpaceCreate(TopSpace); + GroundSpace = d.HashSpaceCreate(TopSpace); + } + catch + { + // i must RtC#FM + } - contactgroup = d.JointGroupCreate(0); + d.HashSpaceSetLevels(TopSpace, -2, 8); + d.HashSpaceSetLevels(ActiveSpace, -2, 8); + d.HashSpaceSetLevels(StaticSpace, -2, 8); + d.HashSpaceSetLevels(GroundSpace, 0, 8); - d.WorldSetAutoDisableFlag(world, false); + // demote to second level + d.SpaceSetSublevel(ActiveSpace, 1); + d.SpaceSetSublevel(StaticSpace, 1); + d.SpaceSetSublevel(GroundSpace, 1); - #if USE_DRAWSTUFF - Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); - viewthread.Start(); - #endif + d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(ActiveSpace, 0); + d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(StaticSpace, 0); - _watermap = new float[258 * 258]; + d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundSpace, 0); - // Zero out the prim spaces array (we split our space into smaller spaces so - // we can hit test less. - } + contactgroup = d.JointGroupCreate(0); + //contactgroup -#if USE_DRAWSTUFF - public void startvisualization(object o) - { - ds.Functions fn; - fn.version = ds.VERSION; - fn.start = new ds.CallbackFunction(start); - fn.step = new ds.CallbackFunction(step); - fn.command = new ds.CallbackFunction(command); - fn.stop = null; - fn.path_to_textures = "./textures"; - string[] args = new string[0]; - ds.SimulationLoop(args.Length, args, 352, 288, ref fn); + d.WorldSetAutoDisableFlag(world, false); + } } -#endif // Initialize the mesh plugin +// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region ) public override void Initialise(IMesher meshmerizer, IConfigSource config) { - InitializeExtraStats(); - +// checkThread(); mesher = meshmerizer; m_config = config; - // Defaults - if (Environment.OSVersion.Platform == PlatformID.Unix) + string ode_config = d.GetConfiguration(); + if (ode_config != null && ode_config != "") { - avPIDD = 3200.0f; - avPIDP = 1400.0f; - avStandupTensor = 2000000f; - } - else - { - avPIDD = 2200.0f; - avPIDP = 900.0f; - avStandupTensor = 550000f; + m_log.WarnFormat("ODE configuration: {0}", ode_config); + + if (ode_config.Contains("ODE_Ubit")) + { + OdeUbitLib = true; + } } + /* + if (region != null) + { + WorldExtents.X = region.RegionSizeX; + WorldExtents.Y = region.RegionSizeY; + } + */ + + // Defaults + int contactsPerCollision = 80; + IConfig physicsconfig = null; + if (m_config != null) { - IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + physicsconfig = m_config.Configs["ODEPhysicsSettings"]; if (physicsconfig != null) { - CollectStats = physicsconfig.GetBoolean("collect_stats", false); - - gravityx = physicsconfig.GetFloat("world_gravityx", 0f); - gravityy = physicsconfig.GetFloat("world_gravityy", 0f); - gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f); - - float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f); - AvatarTerminalVelocity = Util.Clamp(avatarTerminalVelocity, 0, 255f); - if (AvatarTerminalVelocity != avatarTerminalVelocity) - { - m_log.WarnFormat( - "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid. Clamping to {1}", - avatarTerminalVelocity, AvatarTerminalVelocity); - } - - worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4); - worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128); - - metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); - smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4); - smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66); + gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); + gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); + gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); - contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f); + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); - nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f); - nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f); - nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f); - - mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f); - mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f); - mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f); - - nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f); - nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f); - - mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f); - mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f); + contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); - m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); + m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations); - avDensity = physicsconfig.GetFloat("av_density", 80f); -// avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f); - avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); - avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); - avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f); - IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false); + avDensity = physicsconfig.GetFloat("av_density", avDensity); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); + avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius); - contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); - geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5); + geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); - geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); - - geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); - bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); +// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); - bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f); - bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f); - - forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); - meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); - meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f); - MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f); - m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false); - - if (Environment.OSVersion.Platform == PlatformID.Unix) - { - avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f); - avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f); - avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f); - bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f); - } - else - { - avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); - avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); - avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f); - bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f); - } + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); + bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); physics_logging = physicsconfig.GetBoolean("physics_logging", false); physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); - m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false); - minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); - maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); + minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); } } - contacts = new d.ContactGeom[contactsPerCollision]; - - staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)]; - - // Centeral contact friction and bounce - // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why - // an avatar falls through in Z but not in X or Y when walking on a prim. - contact.surface.mode |= d.ContactFlags.SoftERP; - contact.surface.mu = nmAvatarObjectContactFriction; - contact.surface.bounce = nmAvatarObjectContactBounce; - contact.surface.soft_cfm = 0.010f; - contact.surface.soft_erp = 0.010f; - - // Terrain contact friction and Bounce - // This is the *non* moving version. Use this when an avatar - // isn't moving to keep it in place better - TerrainContact.surface.mode |= d.ContactFlags.SoftERP; - TerrainContact.surface.mu = nmTerrainContactFriction; - TerrainContact.surface.bounce = nmTerrainContactBounce; - TerrainContact.surface.soft_erp = nmTerrainContactERP; - - WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM); - WaterContact.surface.mu = 0f; // No friction - WaterContact.surface.bounce = 0.0f; // No bounce - WaterContact.surface.soft_cfm = 0.010f; - WaterContact.surface.soft_erp = 0.010f; - - // Prim contact friction and bounce - // THis is the *non* moving version of friction and bounce - // Use this when an avatar comes in contact with a prim - // and is moving - AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction; - AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce; - - // Terrain contact friction bounce and various error correcting calculations - // Use this when an avatar is in contact with the terrain and moving. - AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP; - AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction; - AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce; - AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP; + m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); - /* - - Stone = 0, - /// - Metal = 1, - /// - Glass = 2, - /// - Wood = 3, - /// - Flesh = 4, - /// - Plastic = 5, - /// - Rubber = 6 - */ + HalfOdeStep = ODE_STEPSIZE * 0.5f; + odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); - m_materialContacts = new d.Contact[7,2]; - - m_materialContacts[(int)Material.Stone, 0] = new d.Contact(); - m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Stone, 1] = new d.Contact(); - m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Metal, 0] = new d.Contact(); - m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Metal, 1] = new d.Contact(); - m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Glass, 0] = new d.Contact(); - m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f; - m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f; - m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f; + ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); + GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); - /* - private float nmAvatarObjectContactFriction = 250f; - private float nmAvatarObjectContactBounce = 0.1f; + m_materialContactsData[(int)Material.Stone].mu = 0.8f; + m_materialContactsData[(int)Material.Stone].bounce = 0.4f; - private float mAvatarObjectContactFriction = 75f; - private float mAvatarObjectContactBounce = 0.1f; - */ - m_materialContacts[(int)Material.Glass, 1] = new d.Contact(); - m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f; - m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f; - m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Wood, 0] = new d.Contact(); - m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Wood, 1] = new d.Contact(); - m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Flesh, 0] = new d.Contact(); - m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Flesh, 1] = new d.Contact(); - m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Plastic, 0] = new d.Contact(); - m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Plastic, 1] = new d.Contact(); - m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Rubber, 0] = new d.Contact(); - m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction; - m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce; - m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f; - - m_materialContacts[(int)Material.Rubber, 1] = new d.Contact(); - m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP; - m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction; - m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce; - m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f; - m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f; - - d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh); + m_materialContactsData[(int)Material.Metal].mu = 0.3f; + m_materialContactsData[(int)Material.Metal].bounce = 0.4f; + + m_materialContactsData[(int)Material.Glass].mu = 0.2f; + m_materialContactsData[(int)Material.Glass].bounce = 0.7f; + + m_materialContactsData[(int)Material.Wood].mu = 0.6f; + m_materialContactsData[(int)Material.Wood].bounce = 0.5f; + + m_materialContactsData[(int)Material.Flesh].mu = 0.9f; + m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; + + m_materialContactsData[(int)Material.Plastic].mu = 0.4f; + m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; + + m_materialContactsData[(int)Material.Rubber].mu = 0.9f; + m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; + + m_materialContactsData[(int)Material.light].mu = 0.0f; + m_materialContactsData[(int)Material.light].bounce = 0.0f; // Set the gravity,, don't disable things automatically (we set it explicitly on some things) d.WorldSetGravity(world, gravityx, gravityy, gravityz); d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); - d.WorldSetLinearDamping(world, 256f); - d.WorldSetAngularDamping(world, 256f); - d.WorldSetAngularDampingThreshold(world, 256f); - d.WorldSetLinearDampingThreshold(world, 256f); - d.WorldSetMaxAngularSpeed(world, 256f); + d.WorldSetLinearDamping(world, 0.002f); + d.WorldSetAngularDamping(world, 0.002f); + d.WorldSetAngularDampingThreshold(world, 0f); + d.WorldSetLinearDampingThreshold(world, 0f); + d.WorldSetMaxAngularSpeed(world, 100f); + + d.WorldSetCFM(world,1e-6f); // a bit harder than default + //d.WorldSetCFM(world, 1e-4f); // a bit harder than default + d.WorldSetERP(world, 0.6f); // higher than original // Set how many steps we go without running collision testing // This is in addition to the step size. // Essentially Steps * m_physicsiterations d.WorldSetQuickStepNumIterations(world, m_physicsiterations); - //d.WorldSetContactMaxCorrectingVel(world, 1000.0f); - for (int i = 0; i < staticPrimspace.GetLength(0); i++) - { - for (int j = 0; j < staticPrimspace.GetLength(1); j++) + d.WorldSetContactMaxCorrectingVel(world, 60.0f); + + spacesPerMeter = 1 / metersInSpace; + spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); + spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter); + + staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; + + // create all spaces now + int i, j; + IntPtr newspace; + + for (i = 0; i < spaceGridMaxX; i++) + for (j = 0; j < spaceGridMaxY; j++) + { + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(newspace, 0); + + staticPrimspace[i, j] = newspace; + } + // let this now be real maximum values + spaceGridMaxX--; + spaceGridMaxY--; + + // create 4 off world spaces (x<0,x>max,y<0,y>max) + staticPrimspaceOffRegion = new IntPtr[4]; + + for (i = 0; i < 4; i++) { - staticPrimspace[i, j] = IntPtr.Zero; + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(newspace, 0); + + staticPrimspaceOffRegion[i] = newspace; } - } - _worldInitialized = true; + m_lastframe = DateTime.UtcNow; + m_lastMeshExpire = m_lastframe; } -// internal void waitForSpaceUnlock(IntPtr space) -// { -// //if (space != IntPtr.Zero) -// //while (d.SpaceLockQuery(space)) { } // Wait and do nothing -// } - -// /// -// /// Debug space message for printing the space that a prim/avatar is in. -// /// -// /// -// /// Returns which split up space the given position is in. -// public string whichspaceamIin(Vector3 pos) -// { -// return calculateSpaceForGeom(pos).ToString(); -// } + internal void waitForSpaceUnlock(IntPtr space) + { + //if (space != IntPtr.Zero) + //while (d.SpaceLockQuery(space)) { } // Wait and do nothing + } #region Collision Detection - /// - /// Collides two geometries. - /// - /// - /// - /// /param> - /// - /// - /// - private int CollideGeoms( - IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize) + // sets a global contact for a joint for contactgeom , and base contact description) + + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale) { - int count; + if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) + return IntPtr.Zero; - lock (OdeScene.UniversalColliderSyncObject) - { - // We do this inside the lock so that we don't count any delay in acquiring it - if (CollectStats) - m_nativeCollisionStartTick = Util.EnvironmentTickCount(); + float erp = contactGeom.depth; + erp *= erpscale; + if (erp < minERP) + erp = minERP; + else if (erp > MaxERP) + erp = MaxERP; - count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize); - } + float depth = contactGeom.depth * dscale; + if (depth > 0.5f) + depth = 0.5f; - // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably - // negligable - if (CollectStats) - m_stats[ODENativeGeomCollisionFrameMsStatName] - += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); + d.Contact newcontact = new d.Contact(); + newcontact.geom.depth = depth; + newcontact.geom.g1 = contactGeom.g1; + newcontact.geom.g2 = contactGeom.g2; + newcontact.geom.pos = contactGeom.pos; + newcontact.geom.normal = contactGeom.normal; + newcontact.geom.side1 = contactGeom.side1; + newcontact.geom.side2 = contactGeom.side2; - return count; + // this needs bounce also + newcontact.surface.mode = comumContactFlags; + newcontact.surface.mu = mu; + newcontact.surface.bounce = bounce; + newcontact.surface.soft_cfm = cfm; + newcontact.surface.soft_erp = erp; + + IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); + Marshal.StructureToPtr(newcontact, contact, true); + return d.JointCreateContactPtr(world, contactgroup, contact); } - /// - /// Collide two spaces or a space and a geometry. - /// - /// - /// /param> - /// - private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data) + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) { - if (CollectStats) - { - m_inCollisionTiming = true; - m_nativeCollisionStartTick = Util.EnvironmentTickCount(); - } - - d.SpaceCollide2(space1, space2, data, nearCallback); + if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) + return false; - if (CollectStats && m_inCollisionTiming) - { - m_stats[ODENativeSpaceCollisionFrameMsStatName] - += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); - m_inCollisionTiming = false; - } + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; } /// @@ -964,76 +664,50 @@ namespace OpenSim.Region.Physics.OdePlugin /// The space that contains the geoms. Remember, spaces are also geoms /// a geometry or space /// another geometry or space + /// + private void near(IntPtr space, IntPtr g1, IntPtr g2) { - if (CollectStats && m_inCollisionTiming) - { - m_stats[ODENativeSpaceCollisionFrameMsStatName] - += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); - m_inCollisionTiming = false; - } - -// m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space); // no lock here! It's invoked from within Simulate(), which is thread-locked + if (m_global_contactcount >= maxContactsbeforedeath) + return; + // Test if we're colliding a geom with a space. // If so we have to drill down into the space recursively + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) { - if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) - return; - - // Separating static prim geometry spaces. // We'll be calling near recursivly if one // of them is a space to find all of the // contact points in the space try { - CollideSpaces(g1, g2, IntPtr.Zero); + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); } catch (AccessViolationException) { - m_log.Error("[ODE SCENE]: Unable to collide test a space"); + m_log.Warn("[PHYSICS]: Unable to collide test a space"); return; } - //Colliding a space or a geom with a space or a geom. so drill down - - //Collide all geoms in each space.. - //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback); - //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback); + //here one should check collisions of geoms inside a space + // but on each space we only should have geoms that not colide amoung each other + // so we don't dig inside spaces return; } - if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) - return; - + // get geom bodies to check if we already a joint contact + // guess this shouldn't happen now IntPtr b1 = d.GeomGetBody(g1); IntPtr b2 = d.GeomGetBody(g2); // d.GeomClassID id = d.GeomGetClass(g1); - String name1 = null; - String name2 = null; - - if (!geom_name_map.TryGetValue(g1, out name1)) - { - name1 = "null"; - } - if (!geom_name_map.TryGetValue(g2, out name2)) - { - name2 = "null"; - } - - //if (id == d.GeomClassId.TriMeshClass) - //{ - // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2); - //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); - //} - // Figure out how many contact points we have int count = 0; - try { // Colliding Geom To Geom @@ -1045,914 +719,611 @@ namespace OpenSim.Region.Physics.OdePlugin if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; - count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf); +// debug + PhysicsActor dp2; + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + { + d.AABB aabb; + d.GeomGetAABB(g2, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if (x > 60.0f || y > 60.0f || z > 60.0f) + { + if (!actor_name_map.TryGetValue(g2, out dp2)) + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + else + m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", + dp2.Name, dp2.Size, x, y, z, + dp2.Position.ToString(), + dp2.Orientation.ToString(), + dp2.Orientation.Length()); + return; + } + } +// + - // All code after this is only relevant if we have any collisions - if (count <= 0) - return; - if (count > contacts.Length) - m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length); + if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || + d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) + { + int cflags; + unchecked + { + cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); + } + count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + else + count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); } catch (SEHException) { - m_log.Error( - "[ODE SCENE]: 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."); + 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.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message); + m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); return; } + // contacts done + if (count == 0) + return; + + // try get physical actors PhysicsActor p1; PhysicsActor p2; - - p1ExpectedPoints = 0; - p2ExpectedPoints = 0; - + if (!actor_name_map.TryGetValue(g1, out p1)) { - p1 = PANull; + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); + return; } if (!actor_name_map.TryGetValue(g2, out p2)) { - p2 = PANull; + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + return; } - ContactPoint maxDepthContact = new ContactPoint(); - if (p1.CollisionScore + count >= float.MaxValue) + // update actors collision score + if (p1.CollisionScore >= float.MaxValue - count) p1.CollisionScore = 0; p1.CollisionScore += count; - if (p2.CollisionScore + count >= float.MaxValue) + if (p2.CollisionScore >= float.MaxValue - count) p2.CollisionScore = 0; p2.CollisionScore += count; - for (int i = 0; i < count; i++) - { - d.ContactGeom curContact = contacts[i]; - - if (curContact.depth > maxDepthContact.PenetrationDepth) - { - maxDepthContact = new ContactPoint( + // get first contact + d.ContactGeom curContact = new d.ContactGeom(); + if (!GetCurContactGeom(0, ref curContact)) + return; + // for now it's the one with max depth + ContactPoint maxDepthContact = new ContactPoint( new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), curContact.depth - ); - } - - //m_log.Warn("[CCOUNT]: " + count); - IntPtr joint; - // If we're colliding with terrain, use 'TerrainContact' instead of contact. - // allows us to have different settings - - // We only need to test p2 for 'jump crouch purposes' - if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim) - { - // Testing if the collision is at the feet of the avatar + ); + // do volume detection case + if ( + (p1.IsVolumeDtc || p2.IsVolumeDtc)) + { + collision_accounting_events(p1, p2, maxDepthContact); + return; + } - //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); - if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)) - p2.IsColliding = true; - } - else - { - p2.IsColliding = true; - } - - //if ((framecount % m_returncollisions) == 0) + // big messy collision analises - switch (p1.PhysicsActorType) - { - case (int)ActorTypes.Agent: - p1ExpectedPoints = avatarExpectedContacts; - p2.CollidingObj = true; - break; - case (int)ActorTypes.Prim: - if (p1 != null && p1 is OdePrim) - p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts; + Vector3 normoverride = Vector3.Zero; //damm c# - if (p2.Velocity.LengthSquared() > 0.0f) - p2.CollidingObj = true; - break; - case (int)ActorTypes.Unknown: - p2.CollidingGround = true; - break; - default: - p2.CollidingGround = true; - break; - } + float mu = 0; + float bounce = 0; + float cfm = 0.0001f; + float erpscale = 1.0f; + float dscale = 1.0f; + bool IgnoreNegSides = false; - // we don't want prim or avatar to explode + ContactData contactdata1 = new ContactData(0, 0, false); + ContactData contactdata2 = new ContactData(0, 0, false); - #region InterPenetration Handling - Unintended physics explosions -# region disabled code1 + bool dop1foot = false; + bool dop2foot = false; + bool ignore = false; + bool AvanormOverride = false; - if (curContact.depth >= 0.08f) - { - //This is disabled at the moment only because it needs more tweaking - //It will eventually be uncommented - /* - if (contact.depth >= 1.00f) + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Agent: { - //m_log.Debug("[PHYSICS]: " + contact.depth.ToString()); - } + AvanormOverride = true; + Vector3 tmp = p2.Position - p1.Position; + normoverride = p2.Velocity - p1.Velocity; + mu = normoverride.LengthSquared(); - //If you interpenetrate a prim with an agent - if ((p2.PhysicsActorType == (int) ActorTypes.Agent && - p1.PhysicsActorType == (int) ActorTypes.Prim) || - (p1.PhysicsActorType == (int) ActorTypes.Agent && - p2.PhysicsActorType == (int) ActorTypes.Prim)) - { - - //contact.depth = contact.depth * 4.15f; - /* - if (p2.PhysicsActorType == (int) ActorTypes.Agent) + if (mu > 1e-6) { - p2.CollidingObj = true; - contact.depth = 0.003f; - p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f); - OdeCharacter character = (OdeCharacter) p2; - character.SetPidStatus(true); - contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2)); - + mu = 1.0f / (float)Math.Sqrt(mu); + normoverride *= mu; + mu = Vector3.Dot(tmp, normoverride); + if (mu > 0) + normoverride *= -1; } else { - - //contact.depth = 0.0000000f; + tmp.Normalize(); + normoverride = -tmp; } - if (p1.PhysicsActorType == (int) ActorTypes.Agent) - { - p1.CollidingObj = true; - contact.depth = 0.003f; - p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f); - contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2)); - OdeCharacter character = (OdeCharacter)p1; - character.SetPidStatus(true); - } - else + switch (p2.PhysicsActorType) { + case (int)ActorTypes.Agent: + p1.CollidingObj = true; + p2.CollidingObj = true; + break; + + case (int)ActorTypes.Prim: + if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + dop1foot = true; + break; - //contact.depth = 0.0000000f; + default: + ignore = true; // avatar to terrain and water ignored + break; } - - - - } -*/ - // If you interpenetrate a prim with another prim - /* - if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim) - { - #region disabledcode2 - //OdePrim op1 = (OdePrim)p1; - //OdePrim op2 = (OdePrim)p2; - //op1.m_collisionscore++; - //op2.m_collisionscore++; - - //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000) - //{ - //op1.m_taintdisable = true; - //AddPhysicsActorTaint(p1); - //op2.m_taintdisable = true; - //AddPhysicsActorTaint(p2); - //} - - //if (contact.depth >= 0.25f) - //{ - // Don't collide, one or both prim will expld. - - //op1.m_interpenetrationcount++; - //op2.m_interpenetrationcount++; - //interpenetrations_before_disable = 200; - //if (op1.m_interpenetrationcount >= interpenetrations_before_disable) - //{ - //op1.m_taintdisable = true; - //AddPhysicsActorTaint(p1); - //} - //if (op2.m_interpenetrationcount >= interpenetrations_before_disable) - //{ - // op2.m_taintdisable = true; - //AddPhysicsActorTaint(p2); - //} - - //contact.depth = contact.depth / 8f; - //contact.normal = new d.Vector3(0, 0, 1); - //} - //if (op1.m_disabled || op2.m_disabled) - //{ - //Manually disabled objects stay disabled - //contact.depth = 0f; - //} - #endregion + break; } - */ -#endregion - if (curContact.depth >= 1.00f) + + case (int)ActorTypes.Prim: + switch (p2.PhysicsActorType) { - //m_log.Info("[P]: " + contact.depth.ToString()); - if ((p2.PhysicsActorType == (int) ActorTypes.Agent && - p1.PhysicsActorType == (int) ActorTypes.Unknown) || - (p1.PhysicsActorType == (int) ActorTypes.Agent && - p2.PhysicsActorType == (int) ActorTypes.Unknown)) - { - if (p2.PhysicsActorType == (int) ActorTypes.Agent) + case (int)ActorTypes.Agent: + AvanormOverride = true; + + Vector3 tmp = p2.Position - p1.Position; + normoverride = p2.Velocity - p1.Velocity; + mu = normoverride.LengthSquared(); + if (mu > 1e-6) { - if (p2 is OdeCharacter) - { - OdeCharacter character = (OdeCharacter) p2; - - //p2.CollidingObj = true; - curContact.depth = 0.00000003f; - p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f); - curContact.pos = - new d.Vector3(curContact.pos.X + (p1.Size.X/2), - curContact.pos.Y + (p1.Size.Y/2), - curContact.pos.Z + (p1.Size.Z/2)); - character.SetPidStatus(true); - } + mu = 1.0f / (float)Math.Sqrt(mu); + normoverride *= mu; + mu = Vector3.Dot(tmp, normoverride); + if (mu > 0) + normoverride *= -1; + } + else + { + tmp.Normalize(); + normoverride = -tmp; + } + + bounce = 0; + mu = 0; + cfm = 0.0001f; + + dop2foot = true; + if (p1.Velocity.LengthSquared() > 0.0f) + p1.CollidingObj = true; + break; + + case (int)ActorTypes.Prim: + if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) + { + p1.CollidingObj = true; + p2.CollidingObj = true; } + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); + bounce = contactdata1.bounce * contactdata2.bounce; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + cfm = p1.Mass; + if (cfm > p2.Mass) + cfm = p2.Mass; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + if (dscale > 1.0f) + dscale = 1.0f; + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + else if (cfm < 0.00001f) + cfm = 0.00001f; + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + break; - if (p1.PhysicsActorType == (int) ActorTypes.Agent) + case (int)ActorTypes.Ground: + p1.getContactData(ref contactdata1); + bounce = contactdata1.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + p1.CollidingGround = true; + + cfm = p1.Mass; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); + if (dscale > 1.0f) + dscale = 1.0f; + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + else if (cfm < 0.00001f) + cfm = 0.00001f; + + if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) { - if (p1 is OdeCharacter) - { - OdeCharacter character = (OdeCharacter) p1; - - //p2.CollidingObj = true; - curContact.depth = 0.00000003f; - p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f); - curContact.pos = - new d.Vector3(curContact.pos.X + (p1.Size.X/2), - curContact.pos.Y + (p1.Size.Y/2), - curContact.pos.Z + (p1.Size.Z/2)); - character.SetPidStatus(true); - } + if (curContact.side1 > 0) + IgnoreNegSides = true; } - } + break; + + case (int)ActorTypes.Water: + default: + ignore = true; + break; } - } + break; + + case (int)ActorTypes.Ground: + if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + p2.CollidingGround = true; + p2.getContactData(ref contactdata2); + bounce = contactdata2.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); - #endregion + cfm = p2.Mass; + dscale = 10 / cfm; + dscale = (float)Math.Sqrt(dscale); - // Logic for collision handling - // Note, that if *all* contacts are skipped (VolumeDetect) - // The prim still detects (and forwards) collision events but - // appears to be phantom for the world - Boolean skipThisContact = false; + if (dscale > 1.0f) + dscale = 1.0f; - if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect)) - skipThisContact = true; // No collision on volume detect prims + erpscale = cfm * 0.01f; + cfm = 0.0001f / cfm; + if (cfm > 0.01f) + cfm = 0.01f; + else if (cfm < 0.00001f) + cfm = 0.00001f; - if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) - skipThisContact = true; // No collision on volume detect prims + if (curContact.side1 > 0) // should be 2 ? + IgnoreNegSides = true; - if (!skipThisContact && curContact.depth < 0f) - skipThisContact = true; + if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + } + else + ignore = true; + break; + + case (int)ActorTypes.Water: + default: + break; + } + if (ignore) + return; + + IntPtr Joint; - if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType)) - skipThisContact = true; + int i = 0; + while(true) + { + + if (IgnoreNegSides && curContact.side1 < 0) + { + if (++i >= count) + break; - const int maxContactsbeforedeath = 4000; - joint = IntPtr.Zero; + if (!GetCurContactGeom(i, ref curContact)) + break; + } + else - if (!skipThisContact) { - _perloopContact.Add(curContact); - if (name1 == "Terrain" || name2 == "Terrain") + if (AvanormOverride) { - if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && - (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) + if (curContact.depth > 0.3f) { - p2ExpectedPoints = avatarExpectedContacts; - // Avatar is moving on terrain, use the movement terrain contact - AvatarMovementTerrainContact.geom = curContact; - - if (m_global_contactcount < maxContactsbeforedeath) - { - joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact); - m_global_contactcount++; - } + if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) + p1.IsColliding = true; + if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) + p2.IsColliding = true; + curContact.normal.X = normoverride.X; + curContact.normal.Y = normoverride.Y; + curContact.normal.Z = normoverride.Z; } + else { - if (p2.PhysicsActorType == (int)ActorTypes.Agent) - { - p2ExpectedPoints = avatarExpectedContacts; - // Avatar is standing on terrain, use the non moving terrain contact - TerrainContact.geom = curContact; - - if (m_global_contactcount < maxContactsbeforedeath) - { - joint = d.JointCreateContact(world, contactgroup, ref TerrainContact); - m_global_contactcount++; - } - } - else + if (dop1foot) { - if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim) + float sz = p1.Size.Z; + Vector3 vtmp = p1.Position; + float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; + if (ppos > 0f) { - // prim prim contact - // int pj294950 = 0; - int movintYN = 0; - int material = (int) Material.Wood; - // prim terrain contact - if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) - { - movintYN = 1; - } - - if (p2 is OdePrim) + if (!p1.Flying) { - material = ((OdePrim) p2).m_material; - p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; - } - - // Unnessesary because p1 is defined above - //if (p1 is OdePrim) - // { - // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts; - // } - //m_log.DebugFormat("Material: {0}", material); + d.AABB aabb; + d.GeomGetAABB(g2, out aabb); + float tmp = vtmp.Z - sz * .18f; - m_materialContacts[material, movintYN].geom = curContact; - - if (m_global_contactcount < maxContactsbeforedeath) - { - joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); - m_global_contactcount++; + if (aabb.MaxZ < tmp) + { + vtmp.X = curContact.pos.X - vtmp.X; + vtmp.Y = curContact.pos.Y - vtmp.Y; + vtmp.Z = -0.2f; + vtmp.Normalize(); + curContact.normal.X = vtmp.X; + curContact.normal.Y = vtmp.Y; + curContact.normal.Z = vtmp.Z; + } } } else - { - int movintYN = 0; - // prim terrain contact - if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) - { - movintYN = 1; - } - - int material = (int)Material.Wood; - - if (p2 is OdePrim) - { - material = ((OdePrim)p2).m_material; - p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; - } - - //m_log.DebugFormat("Material: {0}", material); - m_materialContacts[material, movintYN].geom = curContact; + p1.IsColliding = true; - if (m_global_contactcount < maxContactsbeforedeath) - { - joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); - m_global_contactcount++; - } - } } - } - //if (p2.PhysicsActorType == (int)ActorTypes.Prim) - //{ - //m_log.Debug("[PHYSICS]: prim contacting with ground"); - //} - } - else if (name1 == "Water" || name2 == "Water") - { - /* - if ((p2.PhysicsActorType == (int) ActorTypes.Prim)) - { - } - else - { - } - */ - //WaterContact.surface.soft_cfm = 0.0000f; - //WaterContact.surface.soft_erp = 0.00000f; - if (curContact.depth > 0.1f) - { - curContact.depth *= 52; - //contact.normal = new d.Vector3(0, 0, 1); - //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f); - } - - WaterContact.geom = curContact; - if (m_global_contactcount < maxContactsbeforedeath) - { - joint = d.JointCreateContact(world, contactgroup, ref WaterContact); - m_global_contactcount++; - } - //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth); - } - else - { - if ((p2.PhysicsActorType == (int)ActorTypes.Agent)) - { - p2ExpectedPoints = avatarExpectedContacts; - if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) + if (dop2foot) { - // Avatar is moving on a prim, use the Movement prim contact - AvatarMovementprimContact.geom = curContact; - - if (m_global_contactcount < maxContactsbeforedeath) + float sz = p2.Size.Z; + Vector3 vtmp = p2.Position; + float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; + if (ppos > 0f) { - joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact); - m_global_contactcount++; - } - } - else - { - // Avatar is standing still on a prim, use the non movement contact - contact.geom = curContact; + if (!p2.Flying) + { + d.AABB aabb; + d.GeomGetAABB(g1, out aabb); + float tmp = vtmp.Z - sz * .18f; - if (m_global_contactcount < maxContactsbeforedeath) - { - joint = d.JointCreateContact(world, contactgroup, ref contact); - m_global_contactcount++; + if (aabb.MaxZ < tmp) + { + vtmp.X = curContact.pos.X - vtmp.X; + vtmp.Y = curContact.pos.Y - vtmp.Y; + vtmp.Z = -0.2f; + vtmp.Normalize(); + curContact.normal.X = vtmp.X; + curContact.normal.Y = vtmp.Y; + curContact.normal.Z = vtmp.Z; + } + } } - } - } - else if (p2.PhysicsActorType == (int)ActorTypes.Prim) - { - //p1.PhysicsActorType - int material = (int)Material.Wood; - - if (p2 is OdePrim) - { - material = ((OdePrim)p2).m_material; - p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; - } - - //m_log.DebugFormat("Material: {0}", material); - m_materialContacts[material, 0].geom = curContact; + else + p2.IsColliding = true; - if (m_global_contactcount < maxContactsbeforedeath) - { - joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]); - m_global_contactcount++; } } } - if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide! - { - d.JointAttach(joint, b1, b2); - m_global_contactcount++; - } - } - - collision_accounting_events(p1, p2, maxDepthContact); - - if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle)) - { - // If there are more then 3 contact points, it's likely - // that we've got a pile of objects, so ... - // We don't want to send out hundreds of terse updates over and over again - // so lets throttle them and send them again after it's somewhat sorted out. - p2.ThrottleUpdates = true; - } - //m_log.Debug(count.ToString()); - //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); - } - } - - private bool checkDupe(d.ContactGeom contactGeom, int atype) - { - if (!m_filterCollisions) - return false; + Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); + d.JointAttach(Joint, b1, b2); - bool result = false; + if (++m_global_contactcount >= maxContactsbeforedeath) + break; - ActorTypes at = (ActorTypes)atype; + if (++i >= count) + break; - foreach (d.ContactGeom contact in _perloopContact) - { - //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2)) - //{ - // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2) - if (at == ActorTypes.Agent) - { - if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) - && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) - && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f))) - { - if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f) - { - //contactGeom.depth *= .00005f; - //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); - // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); - result = true; - break; - } -// else -// { -// //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); -// } - } -// else -// { -// //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); -// //int i = 0; -// } - } - else if (at == ActorTypes.Prim) - { - //d.AABB aabb1 = new d.AABB(); - //d.AABB aabb2 = new d.AABB(); + if (!GetCurContactGeom(i, ref curContact)) + break; - //d.GeomGetAABB(contactGeom.g2, out aabb2); - //d.GeomGetAABB(contactGeom.g1, out aabb1); - //aabb1. - if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f))) + if (curContact.depth > maxDepthContact.PenetrationDepth) { - if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z) - { - if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f) - { - result = true; - break; - } - } - //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); - //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.SurfaceNormal.X = curContact.normal.X; + maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; + maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; + maxDepthContact.PenetrationDepth = curContact.depth; } } } - return result; + collision_accounting_events(p1, p2, maxDepthContact); + +/* + if (notskipedcount > geomContactPointsStartthrottle) + { + // If there are more then 3 contact points, it's likely + // that we've got a pile of objects, so ... + // We don't want to send out hundreds of terse updates over and over again + // so lets throttle them and send them again after it's somewhat sorted out. + this needs checking so out for now + if (b1 != IntPtr.Zero) + p1.ThrottleUpdates = true; + if (b2 != IntPtr.Zero) + p2.ThrottleUpdates = true; + + } + */ } private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) { - // obj1LocalID = 0; - //returncollisions = false; - obj2LocalID = 0; - //ctype = 0; - //cStartStop = 0; - if (!p2.SubscribedEvents() && !p1.SubscribedEvents()) - return; - - switch ((ActorTypes)p2.PhysicsActorType) - { - case ActorTypes.Agent: - cc2 = (OdeCharacter)p2; + uint obj2LocalID = 0; - // obj1LocalID = cc2.m_localID; - switch ((ActorTypes)p1.PhysicsActorType) - { - case ActorTypes.Agent: - cc1 = (OdeCharacter)p1; - obj2LocalID = cc1.LocalID; - cc1.AddCollisionEvent(cc2.LocalID, contact); - //ctype = (int)CollisionCategories.Character; - - //if (cc1.CollidingObj) - //cStartStop = (int)StatusIndicators.Generic; - //else - //cStartStop = (int)StatusIndicators.Start; - - //returncollisions = true; - break; + bool p1events = p1.SubscribedEvents(); + bool p2events = p2.SubscribedEvents(); - case ActorTypes.Prim: - if (p1 is OdePrim) - { - cp1 = (OdePrim) p1; - obj2LocalID = cp1.LocalID; - cp1.AddCollisionEvent(cc2.LocalID, contact); - } - //ctype = (int)CollisionCategories.Geom; + if (p1.IsVolumeDtc) + p2events = false; + if (p2.IsVolumeDtc) + p1events = false; - //if (cp1.CollidingObj) - //cStartStop = (int)StatusIndicators.Generic; - //else - //cStartStop = (int)StatusIndicators.Start; + if (!p2events && !p1events) + return; - //returncollisions = true; - break; + Vector3 vel = Vector3.Zero; + if (p2 != null && p2.IsPhysical) + vel = p2.Velocity; - case ActorTypes.Ground: - case ActorTypes.Unknown: - obj2LocalID = 0; - //ctype = (int)CollisionCategories.Land; - //returncollisions = true; - break; - } + if (p1 != null && p1.IsPhysical) + vel -= p1.Velocity; - cc2.AddCollisionEvent(obj2LocalID, contact); - break; + contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); + switch ((ActorTypes)p1.PhysicsActorType) + { + case ActorTypes.Agent: case ActorTypes.Prim: - - if (p2 is OdePrim) { - cp2 = (OdePrim) p2; - - // obj1LocalID = cp2.m_localID; - switch ((ActorTypes) p1.PhysicsActorType) + switch ((ActorTypes)p2.PhysicsActorType) { case ActorTypes.Agent: - if (p1 is OdeCharacter) - { - cc1 = (OdeCharacter) p1; - obj2LocalID = cc1.LocalID; - cc1.AddCollisionEvent(cp2.LocalID, contact); - //ctype = (int)CollisionCategories.Character; - - //if (cc1.CollidingObj) - //cStartStop = (int)StatusIndicators.Generic; - //else - //cStartStop = (int)StatusIndicators.Start; - //returncollisions = true; - } - break; case ActorTypes.Prim: - - if (p1 is OdePrim) + if (p2events) { - cp1 = (OdePrim) p1; - obj2LocalID = cp1.LocalID; - cp1.AddCollisionEvent(cp2.LocalID, contact); - //ctype = (int)CollisionCategories.Geom; - - //if (cp1.CollidingObj) - //cStartStop = (int)StatusIndicators.Generic; - //else - //cStartStop = (int)StatusIndicators.Start; - - //returncollisions = true; + AddCollisionEventReporting(p2); + p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); } + obj2LocalID = p2.ParentActor.LocalID; break; case ActorTypes.Ground: case ActorTypes.Unknown: + default: obj2LocalID = 0; - //ctype = (int)CollisionCategories.Land; - - //returncollisions = true; break; } - - cp2.AddCollisionEvent(obj2LocalID, contact); + if (p1events) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + AddCollisionEventReporting(p1); + p1.AddCollisionEvent(obj2LocalID, contact); + } + break; } - break; - } - //if (returncollisions) - //{ - - //lock (m_storedCollisions) - //{ - //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString(); - //if (m_storedCollisions.ContainsKey(cDictKey)) - //{ - //sCollisionData objd = m_storedCollisions[cDictKey]; - //objd.NumberOfCollisions += 1; - //objd.lastframe = framecount; - //m_storedCollisions[cDictKey] = objd; - //} - //else - //{ - //sCollisionData objd = new sCollisionData(); - //objd.ColliderLocalId = obj1LocalID; - //objd.CollidedWithLocalId = obj2LocalID; - //objd.CollisionType = ctype; - //objd.NumberOfCollisions = 1; - //objd.lastframe = framecount; - //objd.StatusIndicator = cStartStop; - //m_storedCollisions.Add(cDictKey, objd); - //} - //} - // } - } - - private int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount) - { - /* String name1 = null; - String name2 = null; - - if (!geom_name_map.TryGetValue(trimesh, out name1)) - { - name1 = "null"; - } - if (!geom_name_map.TryGetValue(refObject, out name2)) + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + { + if (p2events && !p2.IsVolumeDtc) { - name2 = "null"; + AddCollisionEventReporting(p2); + p2.AddCollisionEvent(0, contact); } - - m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2); - */ - return 1; - } - - private int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex) - { -// String name1 = null; -// String name2 = null; -// -// if (!geom_name_map.TryGetValue(trimesh, out name1)) -// { -// name1 = "null"; -// } -// -// if (!geom_name_map.TryGetValue(refObject, out name2)) -// { -// name2 = "null"; -// } - - // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex); - - d.Vector3 v0 = new d.Vector3(); - d.Vector3 v1 = new d.Vector3(); - d.Vector3 v2 = new d.Vector3(); - - d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2); - // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z); - - return 1; + break; + } + } } /// /// This is our collision testing routine in ODE /// + /// private void collision_optimized() { - _perloopContact.Clear(); - - foreach (OdeCharacter chr in _characters) - { - // Reset the collision values to false - // since we don't know if we're colliding yet - if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) - continue; - - chr.IsColliding = false; - chr.CollidingGround = false; - chr.CollidingObj = false; - - // Test the avatar's geometry for collision with the space - // This will return near and the space that they are the closest to - // And we'll run this again against the avatar and the space segment - // This will return with a bunch of possible objects in the space segment - // and we'll run it again on all of them. + lock (_characters) + { try { - CollideSpaces(space, chr.Shell, IntPtr.Zero); + foreach (OdeCharacter chr in _characters) + { + if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) + continue; + + chr.IsColliding = false; + // chr.CollidingGround = false; not done here + chr.CollidingObj = false; + // do colisions with static space + d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback); + // no coll with gnd + } } catch (AccessViolationException) { - m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", Name); + m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); } - - //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y); - //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10) - //{ - //chr.Position.Z = terrainheight + 10.0f; - //forcedZ = true; - //} + } - if (CollectStats) + lock (_activeprims) { - m_tempAvatarCollisionsThisFrame = _perloopContact.Count; - m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame; + foreach (OdePrim aprim in _activeprims) + { + aprim.CollisionScore = 0; + aprim.IsColliding = false; + } } - List removeprims = null; - foreach (OdePrim chr in _activeprims) + // collide active prims with static enviroment + lock (_activegroups) { - if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled)) + try { - try + foreach (OdePrim prm in _activegroups) { - lock (chr) + if (!prm.m_outbounds) { - if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false) - { - CollideSpaces(space, chr.prim_geom, IntPtr.Zero); - } - else + if (d.BodyIsEnabled(prm.Body)) { - if (removeprims == null) - { - removeprims = new List(); - } - removeprims.Add(chr); - m_log.Error( - "[ODE SCENE]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!"); + d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); + d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); } } } - catch (AccessViolationException) - { - m_log.Error("[ODE SCENE]: Unable to space collide"); - } } - } - - if (CollectStats) - m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame; - - if (removeprims != null) - { - foreach (OdePrim chr in removeprims) + catch (AccessViolationException) { - _activeprims.Remove(chr); + m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); } } - } - - #endregion - - public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) - { - m_worldOffset = offset; - WorldExtents = new Vector2(extents.X, extents.Y); - m_parentScene = pScene; - } - - // Recovered for use by fly height. Kitto Flora - internal float GetTerrainHeightAtXY(float x, float y) - { - int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - - IntPtr heightFieldGeom = IntPtr.Zero; - - if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom)) + // finally colide active things amoung them + try { - if (heightFieldGeom != IntPtr.Zero) - { - if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) - { - - int index; - - - if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y || - (int)x < 0.001f || (int)y < 0.001f) - return 0; - - x = x - offsetX; - y = y - offsetY; - - index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y); - - if (index < TerrainHeightFieldHeights[heightFieldGeom].Length) - { - //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]); - return (float)TerrainHeightFieldHeights[heightFieldGeom][index]; - } - - else - return 0f; - } - else - { - return 0f; - } - - } - else - { - return 0f; - } - + d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); } - else + catch (AccessViolationException) { - return 0f; + m_log.Warn("[PHYSICS]: Unable to collide in Active space"); } - } -// End recovered. Kitto Flora +// _perloopContact.Clear(); + } + #endregion /// /// Add actor to the list that should receive collision events in the simulate loop. /// /// - internal void AddCollisionEventReporting(PhysicsActor obj) + public void AddCollisionEventReporting(PhysicsActor obj) { -// m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID); - - lock (m_collisionEventActorsChanges) - m_collisionEventActorsChanges[obj.LocalID] = obj; + if (!_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Add(obj); } /// /// Remove actor from the list that should receive collision events in the simulate loop. /// /// - internal void RemoveCollisionEventReporting(PhysicsActor obj) + public void RemoveCollisionEventReporting(PhysicsActor obj) + { + if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) + _collisionEventPrimRemove.Add(obj); + } + + public override float TimeDilation { -// m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID); + get { return m_timeDilation; } + } - lock (m_collisionEventActorsChanges) - m_collisionEventActorsChanges[obj.LocalID] = null; + public override bool SupportsNINJAJoints + { + get { return false; } } #region Add/Remove Entities @@ -1963,1266 +1334,613 @@ namespace OpenSim.Region.Physics.OdePlugin pos.X = position.X; pos.Y = position.Y; pos.Z = position.Z; - - OdeCharacter newAv - = new OdeCharacter( - avName, this, pos, size, avPIDD, avPIDP, - avCapRadius, avStandupTensor, avDensity, - avMovementDivisorWalk, avMovementDivisorRun); - + OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); newAv.Flying = isFlying; newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; return newAv; } - public override void RemoveAvatar(PhysicsActor actor) + public void AddCharacter(OdeCharacter chr) { -// m_log.DebugFormat( -// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}", -// actor.Name, actor.LocalID, Name); - - ((OdeCharacter) actor).Destroy(); + lock (_characters) + { + if (!_characters.Contains(chr)) + { + _characters.Add(chr); + if (chr.bad) + m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); + } + } } - internal void AddCharacter(OdeCharacter chr) + public void RemoveCharacter(OdeCharacter chr) { - if (!_characters.Contains(chr)) + lock (_characters) { - _characters.Add(chr); - -// m_log.DebugFormat( -// "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}. Count now {3}", -// chr.Name, chr.LocalID, Name, _characters.Count); - - if (chr.bad) - m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid); + if (_characters.Contains(chr)) + { + _characters.Remove(chr); + } } - else + } + + public void BadCharacter(OdeCharacter chr) + { + lock (_badCharacter) { - m_log.ErrorFormat( - "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!", - chr.Name, chr.LocalID); + if (!_badCharacter.Contains(chr)) + _badCharacter.Add(chr); } } - internal void RemoveCharacter(OdeCharacter chr) + public override void RemoveAvatar(PhysicsActor actor) { - if (_characters.Contains(chr)) - { - _characters.Remove(chr); + //m_log.Debug("[PHYSICS]:ODELOCK"); + ((OdeCharacter) actor).Destroy(); + } + -// m_log.DebugFormat( -// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}. Count now {3}", -// chr.Name, chr.LocalID, Name, _characters.Count); + public void addActivePrim(OdePrim activatePrim) + { + // adds active prim.. + lock (_activeprims) + { + if (!_activeprims.Contains(activatePrim)) + _activeprims.Add(activatePrim); } - else + } + + public void addActiveGroups(OdePrim activatePrim) + { + lock (_activegroups) { - m_log.ErrorFormat( - "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!", - chr.Name, chr.LocalID); + if (!_activegroups.Contains(activatePrim)) + _activegroups.Add(activatePrim); } } private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - PrimitiveBaseShape pbs, bool isphysical, uint localID) + PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) { - Vector3 pos = position; - Vector3 siz = size; - Quaternion rot = rotation; - OdePrim newPrim; lock (OdeLock) { - newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); - + newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); lock (_prims) _prims.Add(newPrim); } - newPrim.LocalID = localID; return newPrim; } - /// - /// Make this prim subject to physics. - /// - /// - internal void ActivatePrim(OdePrim prim) + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) { - // adds active prim.. (ones that should be iterated over in collisions_optimized - if (!_activeprims.Contains(prim)) - _activeprims.Add(prim); - //else - // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); + return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); } + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid) { -// m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name); - - return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid); + return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); } - public override float TimeDilation + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) { - get { return m_timeDilation; } + + return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); } - public override bool SupportsNINJAJoints + public void remActivePrim(OdePrim deactivatePrim) { - get { return m_NINJA_physics_joints_enabled; } + lock (_activeprims) + { + _activeprims.Remove(deactivatePrim); + } } - - // internal utility function: must be called within a lock (OdeLock) - private void InternalAddActiveJoint(PhysicsJoint joint) + public void remActiveGroup(OdePrim deactivatePrim) { - activeJoints.Add(joint); - SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint); + lock (_activegroups) + { + _activegroups.Remove(deactivatePrim); + } } - // internal utility function: must be called within a lock (OdeLock) - private void InternalAddPendingJoint(OdePhysicsJoint joint) + public override void RemovePrim(PhysicsActor prim) { - pendingJoints.Add(joint); - SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint); + // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be + // removed in the next physics simulate pass. + if (prim is OdePrim) + { +// lock (OdeLock) + { + + OdePrim p = (OdePrim)prim; + p.setPrimForRemoval(); + } + } } - - // internal utility function: must be called within a lock (OdeLock) - private void InternalRemovePendingJoint(PhysicsJoint joint) + /// + /// This is called from within simulate but outside the locked portion + /// We need to do our own locking here + /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in + /// Simulate() -- justincc). + /// + /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. + /// + /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory + /// that the space was using. + /// + /// + public void RemovePrimThreadLocked(OdePrim prim) { - pendingJoints.Remove(joint); - SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene); + //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); + lock (prim) + { +// RemoveCollisionEventReporting(prim); + lock (_prims) + _prims.Remove(prim); + } + } - // internal utility function: must be called within a lock (OdeLock) - private void InternalRemoveActiveJoint(PhysicsJoint joint) + public bool havePrim(OdePrim prm) { - activeJoints.Remove(joint); - SOPName_to_activeJoint.Remove(joint.ObjectNameInScene); + lock (_prims) + return _prims.Contains(prm); } - public override void DumpJointInfo() + public bool haveActor(PhysicsActor actor) { - string hdr = "[NINJA] JOINTINFO: "; - foreach (PhysicsJoint j in pendingJoints) - { - m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); - } - m_log.Debug(hdr + pendingJoints.Count + " total pending joints"); - foreach (string jointName in SOPName_to_pendingJoint.Keys) - { - m_log.Debug(hdr + " pending joints dict contains Name: " + jointName); - } - m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries"); - foreach (PhysicsJoint j in activeJoints) - { - m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); - } - m_log.Debug(hdr + activeJoints.Count + " total active joints"); - foreach (string jointName in SOPName_to_activeJoint.Keys) + if (actor is OdePrim) { - m_log.Debug(hdr + " active joints dict contains Name: " + jointName); + lock (_prims) + return _prims.Contains((OdePrim)actor); } - m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries"); - - m_log.Debug(hdr + " Per-body joint connectivity information follows."); - m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints."); - foreach (string actorName in joints_connecting_actor.Keys) + else if (actor is OdeCharacter) { - m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it"); - foreach (PhysicsJoint j in joints_connecting_actor[actorName]) - { - m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); - } - m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor"); + lock (_characters) + return _characters.Contains((OdeCharacter)actor); } + return false; } - public override void RequestJointDeletion(string ObjectNameInScene) - { - lock (externalJointRequestsLock) - { - if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously - { - requestedJointsToBeDeleted.Add(ObjectNameInScene); - } - } - } + #endregion + + #region Space Separation Calculation - private void DeleteRequestedJoints() + /// + /// Called when a static prim moves or becomes static + /// Places the prim in a space one the static sub-spaces grid + /// + /// the pointer to the geom that moved + /// the position that the geom moved to + /// a pointer to the space it was in before it was moved. + /// a pointer to the new space it's in + public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) { - List myRequestedJointsToBeDeleted; - lock (externalJointRequestsLock) - { - // make a local copy of the shared list for processing (threading issues) - myRequestedJointsToBeDeleted = new List(requestedJointsToBeDeleted); - } + // moves a prim into another static sub-space or from another space into a static sub-space - foreach (string jointName in myRequestedJointsToBeDeleted) + // Called ODEPrim so + // it's already in locked space. + + if (geom == IntPtr.Zero) // shouldn't happen + return IntPtr.Zero; + + // get the static sub-space for current position + IntPtr newspace = calculateSpaceForGeom(pos); + + if (newspace == currentspace) // if we are there all done + return newspace; + + // else remove it from its current space + if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) { - lock (OdeLock) + if (d.GeomIsSpace(currentspace)) { - //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName); - if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName)) - { - OdePhysicsJoint joint = null; - if (SOPName_to_activeJoint.ContainsKey(jointName)) - { - joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint; - InternalRemoveActiveJoint(joint); - } - else if (SOPName_to_pendingJoint.ContainsKey(jointName)) - { - joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint; - InternalRemovePendingJoint(joint); - } - - if (joint != null) - { - //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames); - for (int iBodyName = 0; iBodyName < 2; iBodyName++) - { - string bodyName = joint.BodyNames[iBodyName]; - if (bodyName != "NULL") - { - joints_connecting_actor[bodyName].Remove(joint); - if (joints_connecting_actor[bodyName].Count == 0) - { - joints_connecting_actor.Remove(bodyName); - } - } - } + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); - DoJointDeactivated(joint); - if (joint.jointID != IntPtr.Zero) - { - d.JointDestroy(joint.jointID); - joint.jointID = IntPtr.Zero; - //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName); - } - else - { - //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName); - } - } - else - { - // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName); - } - } - else + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) { - // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName); + d.SpaceDestroy(currentspace); } } - } - - // remove processed joints from the shared list - lock (externalJointRequestsLock) - { - foreach (string jointName in myRequestedJointsToBeDeleted) + else { - requestedJointsToBeDeleted.Remove(jointName); + m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + + " Geom:" + geom); } } - } - - // for pending joints we don't know if their associated bodies exist yet or not. - // the joint is actually created during processing of the taints - private void CreateRequestedJoints() - { - List myRequestedJointsToBeCreated; - lock (externalJointRequestsLock) - { - // make a local copy of the shared list for processing (threading issues) - myRequestedJointsToBeCreated = new List(requestedJointsToBeCreated); - } - - foreach (PhysicsJoint joint in myRequestedJointsToBeCreated) + else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space { - lock (OdeLock) + currentspace = d.GeomGetSpace(geom); + if (currentspace != IntPtr.Zero) { - if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null) - { - DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation); - continue; - } - if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null) + if (d.GeomIsSpace(currentspace)) { - DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation); - continue; - } - - InternalAddPendingJoint(joint as OdePhysicsJoint); + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); - if (joint.BodyNames.Count >= 2) - { - for (int iBodyName = 0; iBodyName < 2; iBodyName++) + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) { - string bodyName = joint.BodyNames[iBodyName]; - if (bodyName != "NULL") - { - if (!joints_connecting_actor.ContainsKey(bodyName)) - { - joints_connecting_actor.Add(bodyName, new List()); - } - joints_connecting_actor[bodyName].Add(joint); - } + d.SpaceDestroy(currentspace); } + } } } - // remove processed joints from shared list - lock (externalJointRequestsLock) - { - foreach (PhysicsJoint joint in myRequestedJointsToBeCreated) - { - requestedJointsToBeCreated.Remove(joint); - } - } + // put the geom in the newspace + waitForSpaceUnlock(newspace); + d.SpaceAdd(newspace, geom); + + // let caller know this newspace + return newspace; } /// - /// Add a request for joint creation. + /// Calculates the space the prim should be in by its position /// - /// - /// this joint will just be added to a waiting list that is NOT processed during the main - /// Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests. - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - public override PhysicsJoint RequestJointCreation( - string objectNameInScene, PhysicsJointType jointType, Vector3 position, - Quaternion rotation, string parms, List bodyNames, string trackedBodyName, Quaternion localRotation) + /// + /// a pointer to the space. This could be a new space or reused space. + public IntPtr calculateSpaceForGeom(Vector3 pos) { - OdePhysicsJoint joint = new OdePhysicsJoint(); - joint.ObjectNameInScene = objectNameInScene; - joint.Type = jointType; - joint.Position = position; - joint.Rotation = rotation; - joint.RawParams = parms; - joint.BodyNames = new List(bodyNames); - joint.TrackedBodyName = trackedBodyName; - joint.LocalRotation = localRotation; - joint.jointID = IntPtr.Zero; - joint.ErrorMessageCount = 0; - - lock (externalJointRequestsLock) - { - if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice - { - requestedJointsToBeCreated.Add(joint); - } - } + int x, y; - return joint; - } - - private void RemoveAllJointsConnectedToActor(PhysicsActor actor) - { - //m_log.Debug("RemoveAllJointsConnectedToActor: start"); - if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null) - { - List jointsToRemove = new List(); - //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism) - foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName]) - { - jointsToRemove.Add(j); - } - foreach (PhysicsJoint j in jointsToRemove) - { - //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene); - RequestJointDeletion(j.ObjectNameInScene); - //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene); - j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing) - } - } - } + if (pos.X < 0) + return staticPrimspaceOffRegion[0]; - public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor) - { - //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start"); - lock (OdeLock) - { - //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock"); - RemoveAllJointsConnectedToActor(actor); - } - } + if (pos.Y < 0) + return staticPrimspaceOffRegion[2]; - // normally called from within OnJointMoved, which is called from within a lock (OdeLock) - public override Vector3 GetJointAnchor(PhysicsJoint joint) - { - Debug.Assert(joint.IsInPhysicsEngine); - d.Vector3 pos = new d.Vector3(); + x = (int)(pos.X * spacesPerMeter); + if (x > spaceGridMaxX) + return staticPrimspaceOffRegion[1]; + + y = (int)(pos.Y * spacesPerMeter); + if (y > spaceGridMaxY) + return staticPrimspaceOffRegion[3]; - if (!(joint is OdePhysicsJoint)) - { - DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); - } - else - { - OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint; - switch (odeJoint.Type) - { - case PhysicsJointType.Ball: - d.JointGetBallAnchor(odeJoint.jointID, out pos); - break; - case PhysicsJointType.Hinge: - d.JointGetHingeAnchor(odeJoint.jointID, out pos); - break; - } - } - return new Vector3(pos.X, pos.Y, pos.Z); + return staticPrimspace[x, y]; } + + #endregion - /// - /// Get joint axis. - /// - /// - /// normally called from within OnJointMoved, which is called from within a lock (OdeLock) - /// WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function - /// appears to be unreliable. Fortunately we can compute the joint axis ourselves by - /// keeping track of the joint's original orientation relative to one of the involved bodies. - /// - /// - /// - public override Vector3 GetJointAxis(PhysicsJoint joint) - { - Debug.Assert(joint.IsInPhysicsEngine); - d.Vector3 axis = new d.Vector3(); - - if (!(joint is OdePhysicsJoint)) - { - DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); - } - else - { - OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint; - switch (odeJoint.Type) - { - case PhysicsJointType.Ball: - DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene); - break; - case PhysicsJointType.Hinge: - d.JointGetHingeAxis(odeJoint.jointID, out axis); - break; - } - } - return new Vector3(axis.X, axis.Y, axis.Z); - } /// - /// Stop this prim being subject to physics + /// Called to queue a change to a actor + /// to use in place of old taint mechanism so changes do have a time sequence /// - /// - internal void DeactivatePrim(OdePrim prim) - { - _activeprims.Remove(prim); - } - public override void RemovePrim(PhysicsActor prim) + public void AddChange(PhysicsActor actor, changes what, Object arg) { - // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be - // removed in the next physics simulate pass. - if (prim is OdePrim) - { - lock (OdeLock) - { - OdePrim p = (OdePrim) prim; - - p.setPrimForRemoval(); - AddPhysicsActorTaint(prim); - } - } + ODEchangeitem item = new ODEchangeitem(); + item.actor = actor; + item.what = what; + item.arg = arg; + ChangesQueue.Enqueue(item); } /// - /// This is called from within simulate but outside the locked portion - /// We need to do our own locking here - /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in - /// Simulate() -- justincc). - /// - /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. - /// - /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory - /// that the space was using. + /// Called after our prim properties are set Scale, position etc. + /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex + /// This assures us that we have no race conditions /// /// - internal void RemovePrimThreadLocked(OdePrim prim) + public override void AddPhysicsActorTaint(PhysicsActor prim) { -// m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID); - - lock (prim) - { - RemoveCollisionEventReporting(prim); - - if (prim.prim_geom != IntPtr.Zero) - { - prim.ResetTaints(); - - if (prim.IsPhysical) - { - prim.disableBody(); - if (prim.childPrim) - { - prim.childPrim = false; - prim.Body = IntPtr.Zero; - prim.m_disabled = true; - prim.IsPhysical = false; - } - - - } - // we don't want to remove the main space - - // If the geometry is in the targetspace, remove it from the target space - //m_log.Warn(prim.m_targetSpace); - - //if (prim.m_targetSpace != IntPtr.Zero) - //{ - //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom)) - //{ - - //if (d.GeomIsSpace(prim.m_targetSpace)) - //{ - //waitForSpaceUnlock(prim.m_targetSpace); - //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom); - prim.m_targetSpace = IntPtr.Zero; - //} - //else - //{ - // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + - //((OdePrim)prim).m_targetSpace.ToString()); - //} - - //} - //} - //m_log.Warn(prim.prim_geom); - - if (!prim.RemoveGeom()) - m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene"); - - lock (_prims) - _prims.Remove(prim); - - //If there are no more geometries in the sub-space, we don't need it in the main space anymore - //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0) - //{ - //if (prim.m_targetSpace != null) - //{ - //if (d.GeomIsSpace(prim.m_targetSpace)) - //{ - //waitForSpaceUnlock(prim.m_targetSpace); - //d.SpaceRemove(space, prim.m_targetSpace); - // free up memory used by the space. - //d.SpaceDestroy(prim.m_targetSpace); - //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position); - //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]); - //} - //else - //{ - //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + - //((OdePrim) prim).m_targetSpace.ToString()); - //} - //} - //} - - if (SupportsNINJAJoints) - RemoveAllJointsConnectedToActorThreadLocked(prim); - } - } } - #endregion - - #region Space Separation Calculation - - /// - /// Takes a space pointer and zeros out the array we're using to hold the spaces - /// - /// - private void resetSpaceArrayItemToZero(IntPtr pSpace) + // does all pending changes generated during region load process + public override void PrepareSimulation() { - for (int x = 0; x < staticPrimspace.GetLength(0); x++) + lock (OdeLock) { - for (int y = 0; y < staticPrimspace.GetLength(1); y++) + if (world == IntPtr.Zero) { - if (staticPrimspace[x, y] == pSpace) - staticPrimspace[x, y] = IntPtr.Zero; + ChangesQueue.Clear(); + return; } - } - } - -// private void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY) -// { -// staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero; -// } - - /// - /// Called when a static prim moves. Allocates a space for the prim based on its position - /// - /// the pointer to the geom that moved - /// the position that the geom moved to - /// a pointer to the space it was in before it was moved. - /// a pointer to the new space it's in - internal IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace) - { - // Called from setting the Position and Size of an ODEPrim so - // it's already in locked space. - // we don't want to remove the main space - // we don't need to test physical here because this function should - // never be called if the prim is physical(active) + ODEchangeitem item; - // All physical prim end up in the root space - //Thread.Sleep(20); - if (currentspace != space) - { - //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString()); - //if (currentspace == IntPtr.Zero) - //{ - //int adfadf = 0; - //} - if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero) - { - if (d.GeomIsSpace(currentspace)) - { -// waitForSpaceUnlock(currentspace); - d.SpaceRemove(currentspace, geom); - } - else - { - m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + currentspace + - " Geom:" + geom); - } - } - else + int donechanges = 0; + if (ChangesQueue.Count > 0) { - IntPtr sGeomIsIn = d.GeomGetSpace(geom); - if (sGeomIsIn != IntPtr.Zero) - { - if (d.GeomIsSpace(currentspace)) - { -// waitForSpaceUnlock(sGeomIsIn); - d.SpaceRemove(sGeomIsIn, geom); - } - else - { - m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + - sGeomIsIn + " Geom:" + geom); - } - } - } + m_log.InfoFormat("[ODE] start processing pending actor operations"); + int tstart = Util.EnvironmentTickCount(); - //If there are no more geometries in the sub-space, we don't need it in the main space anymore - if (d.SpaceGetNumGeoms(currentspace) == 0) - { - if (currentspace != IntPtr.Zero) + while (ChangesQueue.Dequeue(out item)) { - if (d.GeomIsSpace(currentspace)) - { -// waitForSpaceUnlock(currentspace); -// waitForSpaceUnlock(space); - d.SpaceRemove(space, currentspace); - // free up memory used by the space. - - //d.SpaceDestroy(currentspace); - resetSpaceArrayItemToZero(currentspace); - } - else + if (item.actor != null) { - m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + - currentspace + " Geom:" + geom); - } - } - } - } - else - { - // this is a physical object that got disabled. ;.; - if (currentspace != IntPtr.Zero && geom != IntPtr.Zero) - { - if (d.SpaceQuery(currentspace, geom)) - { - if (d.GeomIsSpace(currentspace)) - { -// waitForSpaceUnlock(currentspace); - d.SpaceRemove(currentspace, geom); - } - else - { - m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + - currentspace + " Geom:" + geom); - } - } - else - { - IntPtr sGeomIsIn = d.GeomGetSpace(geom); - if (sGeomIsIn != IntPtr.Zero) - { - if (d.GeomIsSpace(sGeomIsIn)) + try { -// waitForSpaceUnlock(sGeomIsIn); - d.SpaceRemove(sGeomIsIn, geom); + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); } - else + catch { - m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + - sGeomIsIn + " Geom:" + geom); + m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); } } + donechanges++; } - } - } - - // The routines in the Position and Size sections do the 'inserting' into the space, - // so all we have to do is make sure that the space that we're putting the prim into - // is in the 'main' space. - int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos); - IntPtr newspace = calculateSpaceForGeom(pos); - - if (newspace == IntPtr.Zero) - { - newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); - d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh); - } - - return newspace; - } - - /// - /// Creates a new space at X Y - /// - /// - /// - /// A pointer to the created space - internal IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY) - { - // creating a new space for prim and inserting it into main space. - staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero); - d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space); -// waitForSpaceUnlock(space); - d.SpaceSetSublevel(space, 1); - d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]); - - return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]; - } - - /// - /// Calculates the space the prim should be in by its position - /// - /// - /// a pointer to the space. This could be a new space or reused space. - internal IntPtr calculateSpaceForGeom(Vector3 pos) - { - int[] xyspace = calculateSpaceArrayItemFromPos(pos); - //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString()); - return staticPrimspace[xyspace[0], xyspace[1]]; - } - - /// - /// Holds the space allocation logic - /// - /// - /// an array item based on the position - internal int[] calculateSpaceArrayItemFromPos(Vector3 pos) - { - int[] returnint = new int[2]; - - returnint[0] = (int) (pos.X/metersInSpace); - - if (returnint[0] > ((int) (259f/metersInSpace))) - returnint[0] = ((int) (259f/metersInSpace)); - if (returnint[0] < 0) - returnint[0] = 0; - - returnint[1] = (int) (pos.Y/metersInSpace); - if (returnint[1] > ((int) (259f/metersInSpace))) - returnint[1] = ((int) (259f/metersInSpace)); - if (returnint[1] < 0) - returnint[1] = 0; - - return returnint; - } - - #endregion - - /// - /// Routine to figure out if we need to mesh this prim with our mesher - /// - /// - /// - internal bool needsMeshing(PrimitiveBaseShape pbs) - { - // most of this is redundant now as the mesher will return null if it cant mesh a prim - // but we still need to check for sculptie meshing being enabled so this is the most - // convenient place to do it for now... - - // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f) - // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString()); - int iPropertiesNotSupportedDefault = 0; - - if (pbs.SculptEntry && !meshSculptedPrim) - { -#if SPAM - m_log.Warn("NonMesh"); -#endif - return false; - } - - // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim - if (!forceSimplePrimMeshing && !pbs.SculptEntry) - { - if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) - || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 - && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) - { - - if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 - && pbs.ProfileHollow == 0 - && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 - && pbs.PathBegin == 0 && pbs.PathEnd == 0 - && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 - && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 - && pbs.PathShearX == 0 && pbs.PathShearY == 0) - { -#if SPAM - m_log.Warn("NonMesh"); -#endif - return false; - } - } - } - - if (pbs.ProfileHollow != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) - iPropertiesNotSupportedDefault++; - - if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) - iPropertiesNotSupportedDefault++; - - // test for torus - if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) - { - if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) - { - if (pbs.PathCurve == (byte)Extrusion.Straight) - { - iPropertiesNotSupportedDefault++; - } - - // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits - else if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) - { - if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) - { - if (pbs.PathCurve == (byte)Extrusion.Straight) - { - iPropertiesNotSupportedDefault++; - } - else if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - - if (pbs.SculptEntry && meshSculptedPrim) - iPropertiesNotSupportedDefault++; - - if (iPropertiesNotSupportedDefault == 0) - { -#if SPAM - m_log.Warn("NonMesh"); -#endif - return false; - } -#if SPAM - m_log.Debug("Mesh"); -#endif - return true; - } - - /// - /// Called after our prim properties are set Scale, position etc. - /// - /// - /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex - /// This assures us that we have no race conditions - /// - /// - public override void AddPhysicsActorTaint(PhysicsActor actor) - { - if (actor is OdePrim) - { - OdePrim taintedprim = ((OdePrim)actor); - lock (_taintedPrims) - _taintedPrims.Add(taintedprim); - } - else if (actor is OdeCharacter) - { - OdeCharacter taintedchar = ((OdeCharacter)actor); - lock (_taintedActors) - { - _taintedActors.Add(taintedchar); - if (taintedchar.bad) - m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid); + int time = Util.EnvironmentTickCountSubtract(tstart); + m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); } } } /// /// This is our main simulate loop - /// - /// /// It's thread locked by a Mutex in the scene. /// It holds Collisions, it instructs ODE to step through the physical reactions /// It moves the objects around in memory /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) - /// + /// /// - /// The number of frames simulated over that period. + /// public override float Simulate(float timeStep) { - if (!_worldInitialized) return 11f; - - int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0; - int tempTick = 0, tempTick2 = 0; - if (framecount >= int.MaxValue) - framecount = 0; - - framecount++; - - float fps = 0; - - float timeLeft = timeStep; - - //m_log.Info(timeStep.ToString()); -// step_time += timeSte -// -// // If We're loaded down by something else, -// // or debugging with the Visual Studio project on pause -// // skip a few frames to catch up gracefully. -// // without shooting the physicsactors all over the place -// -// if (step_time >= m_SkipFramesAtms) -// { -// // Instead of trying to catch up, it'll do 5 physics frames only -// step_time = ODE_STEPSIZE; -// m_physicsiterations = 5; -// } -// else -// { -// m_physicsiterations = 10; -// } - - // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential - // deadlock if the collision event tries to lock something else later on which is already locked by a - // caller that is adding or removing the collision event. - lock (m_collisionEventActorsChanges) - { - foreach (KeyValuePair kvp in m_collisionEventActorsChanges) - { - if (kvp.Value == null) - m_collisionEventActors.Remove(kvp.Key); - else - m_collisionEventActors[kvp.Key] = kvp.Value; - } - - m_collisionEventActorsChanges.Clear(); - } - - if (SupportsNINJAJoints) - { - DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks - CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks - } - - lock (OdeLock) - { - // Process 10 frames if the sim is running normal.. - // process 5 frames if the sim is running slow - //try - //{ - //d.WorldSetQuickStepNumIterations(world, m_physicsiterations); - //} - //catch (StackOverflowException) - //{ - // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); - // ode.drelease(world); - //base.TriggerPhysicsBasedRestart(); - //} - - // Figure out the Frames Per Second we're going at. - //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size - - fps = (timeStep / ODE_STEPSIZE) * 1000; - // HACK: Using a time dilation of 1.0 to debug rubberbanding issues - //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f); - - while (timeLeft > 0.0f) - { - try - { - if (CollectStats) - tempTick = Util.EnvironmentTickCount(); + DateTime now = DateTime.UtcNow; + TimeSpan timedif = now - m_lastframe; + m_lastframe = now; + timeStep = (float)timedif.TotalSeconds; + + // acumulate time so we can reduce error + step_time += timeStep; - lock (_taintedActors) - { - foreach (OdeCharacter character in _taintedActors) - character.ProcessTaints(); + if (step_time < HalfOdeStep) + return 0; - _taintedActors.Clear(); - } + if (framecount < 0) + framecount = 0; - if (CollectStats) - { - tempTick2 = Util.EnvironmentTickCount(); - m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); - tempTick = tempTick2; - } + framecount++; - lock (_taintedPrims) - { - foreach (OdePrim prim in _taintedPrims) - { - if (prim.m_taintremove) - { -// Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name); - RemovePrimThreadLocked(prim); - } - else - { -// Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name); - prim.ProcessTaints(); - } + int curphysiteractions; - prim.m_collisionscore = 0; + // if in trouble reduce step resolution + if (step_time >= m_SkipFramesAtms) + curphysiteractions = m_physicsiterations / 2; + else + curphysiteractions = m_physicsiterations; - // This loop can block up the Heartbeat for a very long time on large regions. - // We need to let the Watchdog know that the Heartbeat is not dead - // NOTE: This is currently commented out, but if things like OAR loading are - // timing the heartbeat out we will need to uncomment it - //Watchdog.UpdateThread(); - } + int nodeframes = 0; - if (SupportsNINJAJoints) - SimulatePendingNINJAJoints(); +// checkThread(); - _taintedPrims.Clear(); - } + lock (SimulationLock) + lock(OdeLock) + { + if (world == IntPtr.Zero) + { + ChangesQueue.Clear(); + return 0; + } - if (CollectStats) - { - tempTick2 = Util.EnvironmentTickCount(); - m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); - tempTick = tempTick2; - } + ODEchangeitem item; - // Move characters - foreach (OdeCharacter actor in _characters) - actor.Move(defects); + if (ChangesQueue.Count > 0) + { + int ttmpstart = Util.EnvironmentTickCount(); + int ttmp; - if (defects.Count != 0) + while (ChangesQueue.Dequeue(out item)) + { + if (item.actor != null) { - foreach (OdeCharacter actor in defects) + try { - m_log.ErrorFormat( - "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving", - actor.Name, actor.LocalID, Name); - - RemoveCharacter(actor); - actor.DestroyOdeStructures(); + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); + } + catch + { + m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); } - - defects.Clear(); } + ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp > 20) + break; + } + } + + d.WorldSetQuickStepNumIterations(world, curphysiteractions); - if (CollectStats) - { - tempTick2 = Util.EnvironmentTickCount(); - m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); - tempTick = tempTick2; - } + while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever + { + try + { + // clear pointer/counter to contacts to pass into joints + m_global_contactcount = 0; - // Move other active objects - foreach (OdePrim prim in _activeprims) + + // Move characters + lock (_characters) { - prim.m_collisionscore = 0; - prim.Move(timeStep); + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(ODE_STEPSIZE, defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + defects.Clear(); + } } - if (CollectStats) + // Move other active objects + lock (_activegroups) { - tempTick2 = Util.EnvironmentTickCount(); - m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); - tempTick = tempTick2; + foreach (OdePrim aprim in _activegroups) + { + aprim.Move(); + } } //if ((framecount % m_randomizeWater) == 0) - // randomizeWater(waterlevel); + // randomizeWater(waterlevel); - //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests(); m_rayCastManager.ProcessQueuedRequests(); - if (CollectStats) - { - tempTick2 = Util.EnvironmentTickCount(); - m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); - tempTick = tempTick2; - } - collision_optimized(); - if (CollectStats) + foreach (PhysicsActor obj in _collisionEventPrim) { - tempTick2 = Util.EnvironmentTickCount(); - m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); - tempTick = tempTick2; - } - - foreach (PhysicsActor obj in m_collisionEventActors.Values) - { -// m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID); + if (obj == null) + continue; switch ((ActorTypes)obj.PhysicsActorType) { case ActorTypes.Agent: OdeCharacter cobj = (OdeCharacter)obj; - cobj.AddCollisionFrameTime(100); + cobj.AddCollisionFrameTime((int)(odetimestepMS)); cobj.SendCollisions(); break; case ActorTypes.Prim: OdePrim pobj = (OdePrim)obj; - pobj.SendCollisions(); + if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) + if (!pobj.m_outbounds) + { + pobj.AddCollisionFrameTime((int)(odetimestepMS)); + pobj.SendCollisions(); + } break; } } -// if (m_global_contactcount > 0) -// m_log.DebugFormat( -// "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount); - - m_global_contactcount = 0; + foreach (PhysicsActor obj in _collisionEventPrimRemove) + _collisionEventPrim.Remove(obj); - if (CollectStats) - { - tempTick2 = Util.EnvironmentTickCount(); - m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); - tempTick = tempTick2; - } + _collisionEventPrimRemove.Clear(); + // do a ode simulation step d.WorldQuickStep(world, ODE_STEPSIZE); + d.JointGroupEmpty(contactgroup); + + // update managed ideia of physical data and do updates to core + /* + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); - if (CollectStats) - m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); + actor.UpdatePositionAndVelocity(); + } + } + } + */ - d.JointGroupEmpty(contactgroup); + lock (_activegroups) + { + { + foreach (OdePrim actor in _activegroups) + { + if (actor.IsPhysical) + { + actor.UpdatePositionAndVelocity(); + } + } + } + } } catch (Exception e) { - m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e); + m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); +// ode.dunlock(world); } - timeLeft -= ODE_STEPSIZE; - } - - if (CollectStats) - tempTick = Util.EnvironmentTickCount(); - foreach (OdeCharacter actor in _characters) - { - if (actor.bad) - m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); - - actor.UpdatePositionAndVelocity(defects); + step_time -= ODE_STEPSIZE; + nodeframes++; } - if (defects.Count != 0) + lock (_badCharacter) { - foreach (OdeCharacter actor in defects) + if (_badCharacter.Count > 0) { - m_log.ErrorFormat( - "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity", - actor.Name, actor.LocalID, Name); + foreach (OdeCharacter chr in _badCharacter) + { + RemoveCharacter(chr); + } - RemoveCharacter(actor); - actor.DestroyOdeStructures(); + _badCharacter.Clear(); } - - defects.Clear(); } - if (CollectStats) + timedif = now - m_lastMeshExpire; + + if (timedif.Seconds > 10) { - tempTick2 = Util.EnvironmentTickCount(); - m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); - tempTick = tempTick2; + mesher.ExpireReleaseMeshs(); + m_lastMeshExpire = now; } - //if (timeStep < 0.2f) +// information block running in debug only +/* + int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); + int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); + int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); + + int nactivegeoms = 0; + int nactivespaces = 0; - foreach (OdePrim prim in _activeprims) + int nstaticgeoms = 0; + int nstaticspaces = 0; + IntPtr sp; + + for (int i = 0; i < ntopactivegeoms; i++) { - if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag)) + sp = d.SpaceGetGeom(ActiveSpace, i); + if (d.GeomIsSpace(sp)) { - prim.UpdatePositionAndVelocity(); - - if (SupportsNINJAJoints) - SimulateActorPendingJoints(prim); + nactivespaces++; + nactivegeoms += d.SpaceGetNumGeoms(sp); } + else + nactivegeoms++; } - if (CollectStats) - m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); + for (int i = 0; i < ntopstaticgeoms; i++) + { + sp = d.SpaceGetGeom(StaticSpace, i); + if (d.GeomIsSpace(sp)) + { + nstaticspaces++; + nstaticgeoms += d.SpaceGetNumGeoms(sp); + } + else + nstaticgeoms++; + } - //DumpJointInfo(); + int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); + int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray + int nbodies = d.NTotalBodies; + int ngeoms = d.NTotalGeoms; +*/ // Finished with all sim stepping. If requested, dump world state to file for debugging. // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? @@ -3241,256 +1959,26 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); } - - latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun); - - // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics - // has a max of 100 ms to run theoretically. - // If the main loop stalls, it calls Simulate later which makes the tick count ms larger. - // If Physics stalls, it takes longer which makes the tick count ms larger. - - if (latertickcount < 100) - { + + // think time dilation as to do with dinamic step size that we dont' have + // even so tell something to world + if (nodeframes < 10) // we did the requested loops m_timeDilation = 1.0f; - } - else + else if (step_time > 0) { - m_timeDilation = 100f / latertickcount; - //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f); + m_timeDilation = timeStep / step_time; + if (m_timeDilation > 1) + m_timeDilation = 1; + if (step_time > m_SkipFramesAtms) + step_time = 0; } - - tickCountFrameRun = Util.EnvironmentTickCount(); - - if (CollectStats) - m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick); } - return fps; - } - - /// - /// Simulate pending NINJA joints. - /// - /// - /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else. - /// - private void SimulatePendingNINJAJoints() - { - // Create pending joints, if possible - - // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating - // a joint requires specifying the body id of both involved bodies - if (pendingJoints.Count > 0) - { - List successfullyProcessedPendingJoints = new List(); - //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints"); - foreach (PhysicsJoint joint in pendingJoints) - { - //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams); - string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries); - List jointBodies = new List(); - bool allJointBodiesAreReady = true; - foreach (string jointParam in jointParams) - { - if (jointParam == "NULL") - { - //DoJointErrorMessage(joint, "attaching NULL joint to world"); - jointBodies.Add(IntPtr.Zero); - } - else - { - //DoJointErrorMessage(joint, "looking for prim name: " + jointParam); - bool foundPrim = false; - lock (_prims) - { - foreach (OdePrim prim in _prims) // FIXME: inefficient - { - if (prim.SOPName == jointParam) - { - //DoJointErrorMessage(joint, "found for prim name: " + jointParam); - if (prim.IsPhysical && prim.Body != IntPtr.Zero) - { - jointBodies.Add(prim.Body); - foundPrim = true; - break; - } - else - { - DoJointErrorMessage(joint, "prim name " + jointParam + - " exists but is not (yet) physical; deferring joint creation. " + - "IsPhysical property is " + prim.IsPhysical + - " and body is " + prim.Body); - foundPrim = false; - break; - } - } - } - } - if (foundPrim) - { - // all is fine - } - else - { - allJointBodiesAreReady = false; - break; - } - } - } - - if (allJointBodiesAreReady) - { - //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams); - if (jointBodies[0] == jointBodies[1]) - { - DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams); - } - else - { - switch (joint.Type) - { - case PhysicsJointType.Ball: - { - IntPtr odeJoint; - //DoJointErrorMessage(joint, "ODE creating ball joint "); - odeJoint = d.JointCreateBall(world, IntPtr.Zero); - //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]); - d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]); - //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position); - d.JointSetBallAnchor(odeJoint, - joint.Position.X, - joint.Position.Y, - joint.Position.Z); - //DoJointErrorMessage(joint, "ODE joint setting OK"); - //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: "); - //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment")); - //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: "); - //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment")); - - if (joint is OdePhysicsJoint) - { - ((OdePhysicsJoint)joint).jointID = odeJoint; - } - else - { - DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!"); - } - } - break; - case PhysicsJointType.Hinge: - { - IntPtr odeJoint; - //DoJointErrorMessage(joint, "ODE creating hinge joint "); - odeJoint = d.JointCreateHinge(world, IntPtr.Zero); - //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]); - d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]); - //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position); - d.JointSetHingeAnchor(odeJoint, - joint.Position.X, - joint.Position.Y, - joint.Position.Z); - // We use the orientation of the x-axis of the joint's coordinate frame - // as the axis for the hinge. - - // Therefore, we must get the joint's coordinate frame based on the - // joint.Rotation field, which originates from the orientation of the - // joint's proxy object in the scene. - - // The joint's coordinate frame is defined as the transformation matrix - // that converts a vector from joint-local coordinates into world coordinates. - // World coordinates are defined as the XYZ coordinate system of the sim, - // as shown in the top status-bar of the viewer. - - // Once we have the joint's coordinate frame, we extract its X axis (AtAxis) - // and use that as the hinge axis. - - //joint.Rotation.Normalize(); - Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation); - - // Now extract the X axis of the joint's coordinate frame. - - // Do not try to use proxyFrame.AtAxis or you will become mired in the - // tar pit of transposed, inverted, and generally messed-up orientations. - // (In other words, Matrix4.AtAxis() is borked.) - // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness - - // Instead, compute the X axis of the coordinate frame by transforming - // the (1,0,0) vector. At least that works. - - //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame); - Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame); - //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis); - //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis); - d.JointSetHingeAxis(odeJoint, - jointAxis.X, - jointAxis.Y, - jointAxis.Z); - //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f); - if (joint is OdePhysicsJoint) - { - ((OdePhysicsJoint)joint).jointID = odeJoint; - } - else - { - DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!"); - } - } - break; - } - successfullyProcessedPendingJoints.Add(joint); - } - } - else - { - DoJointErrorMessage(joint, "joint could not yet be created; still pending"); - } - } - - foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints) - { - //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams); - //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending"); - InternalRemovePendingJoint(successfullyProcessedJoint); - //DoJointErrorMessage(successfullyProcessedJoint, "adding to active"); - InternalAddActiveJoint(successfullyProcessedJoint); - //DoJointErrorMessage(successfullyProcessedJoint, "done"); - } - } +// return nodeframes * ODE_STEPSIZE; // return real simulated time + return 1000 * nodeframes; // return steps for now * 1000 to keep core happy } /// - /// Simulate the joint proxies of a NINJA actor. - /// - /// - /// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there. - /// - /// - private void SimulateActorPendingJoints(OdePrim actor) - { - // If an actor moved, move its joint proxy objects as well. - // There seems to be an event PhysicsActor.OnPositionUpdate that could be used - // for this purpose but it is never called! So we just do the joint - // movement code here. - - if (actor.SOPName != null && - joints_connecting_actor.ContainsKey(actor.SOPName) && - joints_connecting_actor[actor.SOPName] != null && - joints_connecting_actor[actor.SOPName].Count > 0) - { - foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName]) - { - if (affectedJoint.IsInPhysicsEngine) - { - DoJointMoved(affectedJoint); - } - else - { - DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams); - } - } - } - } - public override void GetResults() { } @@ -3498,275 +1986,141 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool IsThreaded { // for now we won't be multithreaded - get { return false; } + get { return (false); } } - #region ODE Specific Terrain Fixes - private float[] ResizeTerrain512NearestNeighbour(float[] heightMap) + public float GetTerrainHeightAtXY(float x, float y) { - float[] returnarr = new float[262144]; - float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y]; - // Filling out the array into its multi-dimensional components - for (int y = 0; y < WorldExtents.Y; y++) - { - for (int x = 0; x < WorldExtents.X; x++) - { - resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x]; - } - } - // Resize using Nearest Neighbour - - // This particular way is quick but it only works on a multiple of the original - - // The idea behind this method can be described with the following diagrams - // second pass and third pass happen in the same loop really.. just separated - // them to show what this does. - - // First Pass - // ResultArr: - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - - // Second Pass - // ResultArr2: - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - - // Third pass fills in the blanks - // ResultArr2: - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - - // X,Y = . - // X+1,y = ^ - // X,Y+1 = * - // X+1,Y+1 = # - - // Filling in like this; - // .* - // ^# - // 1st . - // 2nd * - // 3rd ^ - // 4th # - // on single loop. - - float[,] resultarr2 = new float[512, 512]; - for (int y = 0; y < WorldExtents.Y; y++) - { - for (int x = 0; x < WorldExtents.X; x++) - { - resultarr2[y * 2, x * 2] = resultarr[y, x]; + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - if (y < WorldExtents.Y) - { - resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x]; - } - if (x < WorldExtents.X) - { - resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x]; - } - if (x < WorldExtents.X && y < WorldExtents.Y) - { - resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x]; - } - } - } - //Flatten out the array - int i = 0; - for (int y = 0; y < 512; y++) - { - for (int x = 0; x < 512; x++) - { - if (resultarr2[y, x] <= 0) - returnarr[i] = 0.0000001f; - else - returnarr[i] = resultarr2[y, x]; + IntPtr heightFieldGeom = IntPtr.Zero; - i++; - } - } + // get region map + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return 0f; - return returnarr; - } + if (heightFieldGeom == IntPtr.Zero) + return 0f; - private float[] ResizeTerrain512Interpolation(float[] heightMap) - { - float[] returnarr = new float[262144]; - float[,] resultarr = new float[512,512]; + if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + return 0f; + + // TerrainHeightField for ODE as offset 1m + x += 1f - offsetX; + y += 1f - offsetY; - // Filling out the array into its multi-dimensional components - for (int y = 0; y < 256; y++) + // make position fit into array + if (x < 0) + x = 0; + if (y < 0) + y = 0; + + // integer indexs + int ix; + int iy; + // interpolators offset + float dx; + float dy; + + int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples + + if (OdeUbitLib) { - for (int x = 0; x < 256; x++) + if (x < regsize - 1) + { + ix = (int)x; + dx = x - (float)ix; + } + else // out world use external height + { + ix = regsize - 2; + dx = 0; + } + if (y < regsize - 1) + { + iy = (int)y; + dy = y - (float)iy; + } + else { - resultarr[y, x] = heightMap[y * 256 + x]; + iy = regsize - 2; + dy = 0; } } - // Resize using interpolation - - // This particular way is quick but it only works on a multiple of the original - - // The idea behind this method can be described with the following diagrams - // second pass and third pass happen in the same loop really.. just separated - // them to show what this does. - - // First Pass - // ResultArr: - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - // 1,1,1,1,1,1 - - // Second Pass - // ResultArr2: - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - // ,,,,,,,,,, - // 1,,1,,1,,1,,1,,1, - - // Third pass fills in the blanks - // ResultArr2: - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - // 1,1,1,1,1,1,1,1,1,1,1,1 - - // X,Y = . - // X+1,y = ^ - // X,Y+1 = * - // X+1,Y+1 = # - - // Filling in like this; - // .* - // ^# - // 1st . - // 2nd * - // 3rd ^ - // 4th # - // on single loop. - - float[,] resultarr2 = new float[512,512]; - for (int y = 0; y < (int)Constants.RegionSize; y++) + else { - for (int x = 0; x < (int)Constants.RegionSize; x++) + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) { - resultarr2[y*2, x*2] = resultarr[y, x]; - - if (y < (int)Constants.RegionSize) - { - if (y + 1 < (int)Constants.RegionSize) - { - if (x + 1 < (int)Constants.RegionSize) - { - resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + - resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); - } - else - { - resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2); - } - } - else - { - resultarr2[(y*2) + 1, x*2] = resultarr[y, x]; - } - } - if (x < (int)Constants.RegionSize) - { - if (x + 1 < (int)Constants.RegionSize) - { - if (y + 1 < (int)Constants.RegionSize) - { - resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + - resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); - } - else - { - resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2); - } - } - else - { - resultarr2[y*2, (x*2) + 1] = resultarr[y, x]; - } - } - if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize) - { - if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize)) + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsize - 2; + dy = 0; + } + if (y < regsize - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsize - 2; + dx = 0; + } + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + /* + if ((dx + dy) <= 1.0f) { - resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + - resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + h0 = ((float)heights[iy]); // 0,0 vertice + h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 } else { - resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; + h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice + h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 + h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 } - } - } + */ + h0 = ((float)heights[iy]); // 0,0 vertice + + if ((dy > dx)) + { + iy += regsize; + h2 = (float)heights[iy]; // 0,1 vertice + h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 + h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 } - //Flatten out the array - int i = 0; - for (int y = 0; y < 512; y++) + else { - for (int x = 0; x < 512; x++) - { - if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) - { - m_log.Warn("[ODE SCENE]: Non finite heightfield element detected. Setting it to 0"); - resultarr2[y, x] = 0; - } - returnarr[i] = resultarr2[y, x]; - i++; - } + iy++; + h2 = (float)heights[iy]; // vertice 1,0 + h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 } - return returnarr; + return h0 + h1 + h2; } - #endregion public override void SetTerrain(float[] heightMap) { @@ -3783,78 +2137,75 @@ namespace OpenSim.Region.Physics.OdePlugin } } - private void SetTerrain(float[] heightMap, Vector3 pOffset) + public override void CombineTerrain(float[] heightMap, Vector3 pOffset) { - int startTime = Util.EnvironmentTickCount(); - m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", Name, pOffset); - - // this._heightmap[i] = (double)heightMap[i]; - // dbm (danx0r) -- creating a buffer zone of one extra sample all around - //_origheightmap = heightMap; - - float[] _heightmap; - - // zero out a heightmap array float array (single dimension [flattened])) - //if ((int)Constants.RegionSize == 256) - // _heightmap = new float[514 * 514]; - //else + SetTerrain(heightMap, pOffset); + } - _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; + public void SetTerrain(float[] heightMap, Vector3 pOffset) + { + if (OdeUbitLib) + UbitSetTerrain(heightMap, pOffset); + else + OriSetTerrain(heightMap, pOffset); + } - uint heightmapWidth = Constants.RegionSize + 1; - uint heightmapHeight = Constants.RegionSize + 1; + public void OriSetTerrain(float[] heightMap, Vector3 pOffset) + { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future - uint heightmapWidthSamples; + float[] _heightmap; - uint heightmapHeightSamples; + uint heightmapWidth = Constants.RegionSize + 2; + uint heightmapHeight = Constants.RegionSize + 2; - //if (((int)Constants.RegionSize) == 256) - //{ - // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2; - // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2; - // heightmapWidth++; - // heightmapHeight++; - //} - //else - //{ + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; - heightmapWidthSamples = (uint)Constants.RegionSize + 1; - heightmapHeightSamples = (uint)Constants.RegionSize + 1; - //} + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; const float scale = 1.0f; const float offset = 0.0f; - const float thickness = 0.2f; + const float thickness = 10f; const int wrap = 0; - int regionsize = (int) Constants.RegionSize + 2; - //Double resolution - //if (((int)Constants.RegionSize) == 256) - // heightMap = ResizeTerrain512Interpolation(heightMap); + uint regionsize = Constants.RegionSize; + + float hfmin = float.MaxValue; + float hfmax = float.MinValue; + float val; + uint xx; + uint yy; + uint maxXXYY = regionsize - 1; + // flipping map adding one margin all around so things don't fall in edges - // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256) - // regionsize = 512; + uint xt = 0; + xx = 0; - float hfmin = 2000; - float hfmax = -2000; - - for (int x = 0; x < heightmapWidthSamples; x++) + for (uint x = 0; x < heightmapWidthSamples; x++) { - for (int y = 0; y < heightmapHeightSamples; y++) + if (x > 1 && xx < maxXXYY) + xx++; + yy = 0; + for (uint y = 0; y < heightmapHeightSamples; y++) { - int xx = Util.Clip(x - 1, 0, regionsize - 1); - int yy = Util.Clip(y - 1, 0, regionsize - 1); - - - float val= heightMap[yy * (int)Constants.RegionSize + xx]; - _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val; - - hfmin = (val < hfmin) ? val : hfmin; - hfmax = (val > hfmax) ? val : hfmax; + if (y > 1 && y < maxXXYY) + yy += regionsize; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode + _heightmap[xt + y] = val; + + if (hfmin > val) + hfmin = val; + if (hfmax < val) + hfmax = val; } + xt += heightmapHeightSamples; } - lock (OdeLock) { IntPtr GroundGeom = IntPtr.Zero; @@ -3863,62 +2214,177 @@ namespace OpenSim.Region.Physics.OdePlugin RegionTerrain.Remove(pOffset); if (GroundGeom != IntPtr.Zero) { + actor_name_map.Remove(GroundGeom); + d.GeomDestroy(GroundGeom); + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) - { + { + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); TerrainHeightFieldHeights.Remove(GroundGeom); - } - d.SpaceRemove(space, GroundGeom); - d.GeomDestroy(GroundGeom); + } } - } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1, - (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale, + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, offset, thickness, wrap); + d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); - GroundGeom = d.CreateHeightfield(space, HeightmapData, 1); + + GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) { - d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, 0); + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Terrain"; + pa.PhysicsActorType = (int)ActorTypes.Ground; + actor_name_map[GroundGeom] = pa; + +// geom_name_map[GroundGeom] = "Terrain"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + + q1 = q1 * q2; + + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(GroundGeom, ref R); + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); + RegionTerrain.Add(pOffset, GroundGeom); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); } - geom_name_map[GroundGeom] = "Terrain"; + } + } + + public void UbitSetTerrain(float[] heightMap, Vector3 pOffset) + { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future + + float[] _heightmap; + + uint heightmapWidth = Constants.RegionSize + 2; + uint heightmapHeight = Constants.RegionSize + 2; + + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; + + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; - d.Matrix3 R = new d.Matrix3(); - Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); - Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); - //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); + uint regionsize = Constants.RegionSize; - q1 = q1 * q2; - //q1 = q1 * q3; - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); + float hfmin = float.MaxValue; +// float hfmax = float.MinValue; + float val; - d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); - d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)), (pOffset.Y + ((int)Constants.RegionSize * 0.5f)), 0); - IntPtr testGround = IntPtr.Zero; - if (RegionTerrain.TryGetValue(pOffset, out testGround)) + + uint maxXXYY = regionsize - 1; + // adding one margin all around so things don't fall in edges + + uint xx; + uint yy = 0; + uint yt = 0; + + for (uint y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxXXYY) + yy += regionsize; + xx = 0; + for (uint x = 0; x < heightmapWidthSamples; x++) { - RegionTerrain.Remove(pOffset); + if (x > 1 && x < maxXXYY) + xx++; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode + _heightmap[yt + x] = val; + + if (hfmin > val) + hfmin = val; +// if (hfmax < val) +// hfmax = val; } - RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); - TerrainHeightFieldHeights.Add(GroundGeom,_heightmap); + yt += heightmapWidthSamples; } + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + actor_name_map.Remove(GroundGeom); + d.GeomDestroy(GroundGeom); + + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated) + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + TerrainHeightFieldHeights.Remove(GroundGeom); + } + } + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + + const int wrap = 0; + float thickness = hfmin; + if (thickness < 0) + thickness = 1; + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, + thickness, wrap); + +// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, 0); + + + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Terrain"; + pa.PhysicsActorType = (int)ActorTypes.Ground; + actor_name_map[GroundGeom] = pa; - m_log.DebugFormat( - "[ODE SCENE]: Setting terrain for {0} took {1}ms", Name, Util.EnvironmentTickCountSubtract(startTime)); +// geom_name_map[GroundGeom] = "Terrain"; + + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); + RegionTerrain.Add(pOffset, GroundGeom); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + } + } } + public override void DeleteTerrain() { } - internal float GetWaterLevel() + public float GetWaterLevel() { return waterlevel; } @@ -3927,169 +2393,252 @@ namespace OpenSim.Region.Physics.OdePlugin { return true; } +/* + public override void UnCombine(PhysicsScene pScene) + { + IntPtr localGround = IntPtr.Zero; +// float[] localHeightfield; + bool proceed = false; + List geomDestroyList = new List(); -// public override void UnCombine(PhysicsScene pScene) -// { -// IntPtr localGround = IntPtr.Zero; -//// float[] localHeightfield; -// bool proceed = false; -// List geomDestroyList = new List(); -// -// lock (OdeLock) -// { -// if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) -// { -// foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) -// { -// if (geom == localGround) -// { -//// localHeightfield = TerrainHeightFieldHeights[geom]; -// proceed = true; -// } -// else -// { -// geomDestroyList.Add(geom); -// } -// } -// -// if (proceed) -// { -// m_worldOffset = Vector3.Zero; -// WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); -// m_parentScene = null; -// -// foreach (IntPtr g in geomDestroyList) -// { -// // removingHeightField needs to be done or the garbage collector will -// // collect the terrain data before we tell ODE to destroy it causing -// // memory corruption -// if (TerrainHeightFieldHeights.ContainsKey(g)) -// { -//// float[] removingHeightField = TerrainHeightFieldHeights[g]; -// TerrainHeightFieldHeights.Remove(g); -// -// if (RegionTerrain.ContainsKey(g)) -// { -// RegionTerrain.Remove(g); -// } -// -// d.GeomDestroy(g); -// //removingHeightField = new float[0]; -// } -// } -// -// } -// else -// { -// m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); -// } -// } -// } -// } + lock (OdeLock) + { + if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) + { + foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) + { + if (geom == localGround) + { +// localHeightfield = TerrainHeightFieldHeights[geom]; + proceed = true; + } + else + { + geomDestroyList.Add(geom); + } + } + + if (proceed) + { + m_worldOffset = Vector3.Zero; + WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + m_parentScene = null; + + foreach (IntPtr g in geomDestroyList) + { + // removingHeightField needs to be done or the garbage collector will + // collect the terrain data before we tell ODE to destroy it causing + // memory corruption + if (TerrainHeightFieldHeights.ContainsKey(g)) + { +// float[] removingHeightField = TerrainHeightFieldHeights[g]; + TerrainHeightFieldHeights.Remove(g); + + if (RegionTerrain.ContainsKey(g)) + { + RegionTerrain.Remove(g); + } + + d.GeomDestroy(g); + //removingHeightField = new float[0]; + } + } + } + else + { + m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); + } + } + } + } +*/ public override void SetWaterLevel(float baseheight) { waterlevel = baseheight; - randomizeWater(waterlevel); +// randomizeWater(waterlevel); } - - private void randomizeWater(float baseheight) +/* + public void randomizeWater(float baseheight) { - const uint heightmapWidth = m_regionWidth + 2; - const uint heightmapHeight = m_regionHeight + 2; - const uint heightmapWidthSamples = m_regionWidth + 2; - const uint heightmapHeightSamples = m_regionHeight + 2; + const uint heightmapWidth = Constants.RegionSize + 2; + const uint heightmapHeight = Constants.RegionSize + 2; + const uint heightmapWidthSamples = heightmapWidth + 1; + const uint heightmapHeightSamples = heightmapHeight + 1; + const float scale = 1.0f; const float offset = 0.0f; - const float thickness = 2.9f; const int wrap = 0; - for (int i = 0; i < (258 * 258); i++) + float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples]; + + float maxheigh = float.MinValue; + float minheigh = float.MaxValue; + float val; + for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++) { - _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); - // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); + + val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f); + _watermap[i] = val; + if (maxheigh < val) + maxheigh = val; + if (minheigh > val) + minheigh = val; } + float thickness = minheigh; + lock (OdeLock) { if (WaterGeom != IntPtr.Zero) { - d.SpaceRemove(space, WaterGeom); + actor_name_map.Remove(WaterGeom); + d.GeomDestroy(WaterGeom); + d.GeomHeightfieldDataDestroy(WaterHeightmapData); + WaterGeom = IntPtr.Zero; + WaterHeightmapData = IntPtr.Zero; + if(WaterMapHandler.IsAllocated) + WaterMapHandler.Free(); } - IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, + + WaterHeightmapData = d.GeomHeightfieldDataCreate(); + + WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned); + + d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight, (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, offset, thickness, wrap); - d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); - WaterGeom = d.CreateHeightfield(space, HeightmapData, 1); + d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh); + WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1); if (WaterGeom != IntPtr.Zero) { - d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); - d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); - } + d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water)); + d.GeomSetCollideBits(WaterGeom, 0); - geom_name_map[WaterGeom] = "Water"; - d.Matrix3 R = new d.Matrix3(); + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Water"; + pa.PhysicsActorType = (int)ActorTypes.Water; - Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); - Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); - //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); + actor_name_map[WaterGeom] = pa; +// geom_name_map[WaterGeom] = "Water"; - q1 = q1 * q2; - //q1 = q1 * q3; - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); + d.Matrix3 R = new d.Matrix3(); - d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); - d.GeomSetRotation(WaterGeom, ref R); - d.GeomSetPosition(WaterGeom, 128, 128, 0); + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + q1 = q1 * q2; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(WaterGeom, ref R); + d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0); + } } } - +*/ public override void Dispose() { - _worldInitialized = false; - - m_rayCastManager.Dispose(); - m_rayCastManager = null; + if (m_meshWorker != null) + m_meshWorker.Stop(); lock (OdeLock) { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + lock (_prims) { + ChangesQueue.Clear(); foreach (OdePrim prm in _prims) { - RemovePrim(prm); + prm.DoAChange(changes.Remove, null); + _collisionEventPrim.Remove(prm); + } + _prims.Clear(); + } + + OdeCharacter[] chtorem; + lock (_characters) + { + chtorem = new OdeCharacter[_characters.Count]; + _characters.CopyTo(chtorem); + } + + ChangesQueue.Clear(); + foreach (OdeCharacter ch in chtorem) + ch.DoAChange(changes.Remove, null); + + + foreach (IntPtr GroundGeom in RegionTerrain.Values) + { + if (GroundGeom != IntPtr.Zero) + d.GeomDestroy(GroundGeom); + } + + + RegionTerrain.Clear(); + + if (TerrainHeightFieldHeightsHandlers.Count > 0) + { + foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) + { + if (gch.IsAllocated) + gch.Free(); } } - //foreach (OdeCharacter act in _characters) - //{ - //RemoveAvatar(act); - //} + TerrainHeightFieldHeightsHandlers.Clear(); + TerrainHeightFieldHeights.Clear(); +/* + if (WaterGeom != IntPtr.Zero) + { + d.GeomDestroy(WaterGeom); + WaterGeom = IntPtr.Zero; + if (WaterHeightmapData != IntPtr.Zero) + d.GeomHeightfieldDataDestroy(WaterHeightmapData); + WaterHeightmapData = IntPtr.Zero; + + if (WaterMapHandler.IsAllocated) + WaterMapHandler.Free(); + } +*/ + if (ContactgeomsArray != IntPtr.Zero) + Marshal.FreeHGlobal(ContactgeomsArray); + if (GlobalContactsArray != IntPtr.Zero) + Marshal.FreeHGlobal(GlobalContactsArray); + + d.WorldDestroy(world); + world = IntPtr.Zero; //d.CloseODE(); } - } public override Dictionary GetTopColliders() { - Dictionary topColliders; - + Dictionary returncolliders = new Dictionary(); + int cnt = 0; lock (_prims) { - List orderedPrims = new List(_prims); - orderedPrims.OrderByDescending(p => p.CollisionScore).Take(25); - topColliders = orderedPrims.ToDictionary(p => p.LocalID, p => p.CollisionScore); - - foreach (OdePrim p in _prims) - p.CollisionScore = 0; + foreach (OdePrim prm in _prims) + { + if (prm.CollisionScore > 0) + { + returncolliders.Add(prm.LocalID, prm.CollisionScore); + cnt++; + prm.CollisionScore = 0f; + if (cnt > 25) + { + break; + } + } + } } - - return topColliders; + return returncolliders; } public override bool SupportsRayCast() @@ -4113,6 +2662,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } + // don't like this public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) { ContactResult[] ourResults = null; @@ -4129,182 +2679,107 @@ namespace OpenSim.Region.Physics.OdePlugin waitTime++; } if (ourResults == null) - return new List (); + return new List(); return new List(ourResults); } -#if USE_DRAWSTUFF - // Keyboard callback - public void command(int cmd) + public override bool SuportsRaycastWorldFiltered() { - IntPtr geom; - d.Mass mass; - d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f); - - - - Char ch = Char.ToLower((Char)cmd); - switch ((Char)ch) - { - case 'w': - try - { - Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); - - xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z; - ds.SetViewpoint(ref xyz, ref hpr); - } - catch (ArgumentException) - { hpr.X = 0; } - break; - - case 'a': - hpr.X++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - - case 's': - try - { - Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); - - xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z; - ds.SetViewpoint(ref xyz, ref hpr); - } - catch (ArgumentException) - { hpr.X = 0; } - break; - case 'd': - hpr.X--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'r': - xyz.Z++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'f': - xyz.Z--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'e': - xyz.Y++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'q': - xyz.Y--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - } + return true; } - public void step(int pause) + public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) { - - ds.SetColor(1.0f, 1.0f, 0.0f); - ds.SetTexture(ds.Texture.Wood); - lock (_prims) + object SyncObject = new object(); + List ourresults = new List(); + + RayCallback retMethod = delegate(List results) { - foreach (OdePrim prm in _prims) + lock (SyncObject) { - //IntPtr body = d.GeomGetBody(prm.prim_geom); - if (prm.prim_geom != IntPtr.Zero) - { - d.Vector3 pos; - d.GeomCopyPosition(prm.prim_geom, out pos); - //d.BodyCopyPosition(body, out pos); - - d.Matrix3 R; - d.GeomCopyRotation(prm.prim_geom, out R); - //d.BodyCopyRotation(body, out R); - - - d.Vector3 sides = new d.Vector3(); - sides.X = prm.Size.X; - sides.Y = prm.Size.Y; - sides.Z = prm.Size.Z; - - ds.DrawBox(ref pos, ref R, ref sides); - } + ourresults = results; + Monitor.PulseAll(SyncObject); } - } - ds.SetColor(1.0f, 0.0f, 0.0f); + }; - foreach (OdeCharacter chr in _characters) + lock (SyncObject) { - if (chr.Shell != IntPtr.Zero) - { - IntPtr body = d.GeomGetBody(chr.Shell); - - d.Vector3 pos; - d.GeomCopyPosition(chr.Shell, out pos); - //d.BodyCopyPosition(body, out pos); - - d.Matrix3 R; - d.GeomCopyRotation(chr.Shell, out R); - //d.BodyCopyRotation(body, out R); - - ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f); - d.Vector3 sides = new d.Vector3(); - sides.X = 0.5f; - sides.Y = 0.5f; - sides.Z = 0.5f; - - ds.DrawBox(ref pos, ref R, ref sides); - } + m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod); + if (!Monitor.Wait(SyncObject, 500)) + return null; + else + return ourresults; } } - public void start(int unused) + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) { - ds.SetViewpoint(ref xyz, ref hpr); + if (retMethod != null && actor !=null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return; + if (geom == IntPtr.Zero) + return; + m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod); + } } -#endif - public override Dictionary GetStats() + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { - if (!CollectStats) - return null; - - Dictionary returnStats; - - lock (OdeLock) + if (retMethod != null && actor != null) { - returnStats = new Dictionary(m_stats); - - // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by - // 3 from the SimStatsReporter. - returnStats[ODETotalAvatarsStatName] = _characters.Count * 3; - returnStats[ODETotalPrimsStatName] = _prims.Count * 3; - returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3; + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return; + if (geom == IntPtr.Zero) + return; - InitializeExtraStats(); + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); } - - returnStats[ODEOtherCollisionFrameMsStatName] - = returnStats[ODEOtherCollisionFrameMsStatName] - - returnStats[ODENativeSpaceCollisionFrameMsStatName] - - returnStats[ODENativeGeomCollisionFrameMsStatName]; - - return returnStats; } - private void InitializeExtraStats() + // don't like this + public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) { - m_stats[ODETotalFrameMsStatName] = 0; - m_stats[ODEAvatarTaintMsStatName] = 0; - m_stats[ODEPrimTaintMsStatName] = 0; - m_stats[ODEAvatarForcesFrameMsStatName] = 0; - m_stats[ODEPrimForcesFrameMsStatName] = 0; - m_stats[ODERaycastingFrameMsStatName] = 0; - m_stats[ODENativeStepFrameMsStatName] = 0; - m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0; - m_stats[ODENativeGeomCollisionFrameMsStatName] = 0; - m_stats[ODEOtherCollisionFrameMsStatName] = 0; - m_stats[ODECollisionNotificationFrameMsStatName] = 0; - m_stats[ODEAvatarContactsStatsName] = 0; - m_stats[ODEPrimContactsStatName] = 0; - m_stats[ODEAvatarUpdateFrameMsStatName] = 0; - m_stats[ODEPrimUpdateFrameMsStatName] = 0; + if (actor != null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return new List(); + if (geom == IntPtr.Zero) + return new List(); + + ContactResult[] ourResults = null; + RayCallback retMethod = delegate(List results) + { + ourResults = new ContactResult[results.Count]; + results.CopyTo(ourResults, 0); + }; + int waitTime = 0; + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + while (ourResults == null && waitTime < 1000) + { + Thread.Sleep(1); + waitTime++; + } + if (ourResults == null) + return new List(); + return new List(ourResults); + } + return new List(); } } -} \ No newline at end of file +} -- cgit v1.1 From 6e217965841d474ba7e19c99dcfa5e86c1c459da Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 12 Oct 2012 23:37:28 +0100 Subject: [TEST] disk cache meshs --- OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 551 +++++++++++++--------- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 259 +++++++--- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 4 +- 3 files changed, 532 insertions(+), 282 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index a0a18c4..1e9b8bc 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -32,30 +32,49 @@ using System.Runtime.InteropServices; using OpenSim.Region.Physics.Manager; using PrimMesher; using OpenMetaverse; +using System.Runtime.Serialization; +using System.Runtime.Serialization.Formatters.Binary; namespace OpenSim.Region.Physics.Meshing { - public class Mesh : IMesh + public class MeshBuildingData { + public Dictionary m_vertices; + public List m_triangles; + public float m_obbXmin; + public float m_obbXmax; + public float m_obbYmin; + public float m_obbYmax; + public float m_obbZmin; + public float m_obbZmax; + public Vector3 m_centroid; + public int m_centroidDiv; + } - private Dictionary m_vertices; - private List m_triangles; - GCHandle m_pinnedVertexes; - GCHandle m_pinnedIndex; + [Serializable()] + public class Mesh : IMesh + { + float[] vertices; + int[] indexes; + Vector3 m_obb; + Vector3 m_obboffset; + [NonSerialized()] + MeshBuildingData m_bdata; + [NonSerialized()] + GCHandle vhandler; + [NonSerialized()] + GCHandle ihandler; + [NonSerialized()] IntPtr m_verticesPtr = IntPtr.Zero; - int m_vertexCount = 0; + [NonSerialized()] IntPtr m_indicesPtr = IntPtr.Zero; + [NonSerialized()] + int m_vertexCount = 0; + [NonSerialized()] int m_indexCount = 0; - public float[] m_normals; - Vector3 m_centroid; - float m_obbXmin; - float m_obbXmax; - float m_obbYmin; - float m_obbYmax; - float m_obbZmin; - float m_obbZmax; - int m_centroidDiv; + public int RefCount { get; set; } + public AMeshKey Key { get; set; } private class vertexcomp : IEqualityComparer { @@ -79,42 +98,82 @@ namespace OpenSim.Region.Physics.Meshing { vertexcomp vcomp = new vertexcomp(); - m_vertices = new Dictionary(vcomp); - m_triangles = new List(); - m_centroid = Vector3.Zero; - m_centroidDiv = 0; - m_obbXmin = float.MaxValue; - m_obbXmax = float.MinValue; - m_obbYmin = float.MaxValue; - m_obbYmax = float.MinValue; - m_obbZmin = float.MaxValue; - m_obbZmax = float.MinValue; + m_bdata = new MeshBuildingData(); + m_bdata.m_vertices = new Dictionary(vcomp); + m_bdata.m_triangles = new List(); + m_bdata.m_centroid = Vector3.Zero; + m_bdata.m_centroidDiv = 0; + m_bdata.m_obbXmin = float.MaxValue; + m_bdata.m_obbXmax = float.MinValue; + m_bdata.m_obbYmin = float.MaxValue; + m_bdata.m_obbYmax = float.MinValue; + m_bdata.m_obbZmin = float.MaxValue; + m_bdata.m_obbZmax = float.MinValue; + m_obb = new Vector3(0.5f, 0.5f, 0.5f); + m_obboffset = Vector3.Zero; } - public int RefCount { get; set; } - - public AMeshKey Key { get; set; } - public void Scale(Vector3 scale) + public Mesh Scale(Vector3 scale) { + if (m_verticesPtr == null || m_indicesPtr == null) + return null; + + Mesh result = new Mesh(); + + float x = scale.X; + float y = scale.Y; + float z = scale.Z; + + result.m_obb.X = m_obb.X * x; + result.m_obb.Y = m_obb.Y * y; + result.m_obb.Z = m_obb.Z * z; + result.m_obboffset.X = m_obboffset.X * x; + result.m_obboffset.Y = m_obboffset.Y * y; + result.m_obboffset.Z = m_obboffset.Z * z; + + result.vertices = new float[vertices.Length]; + int j = 0; + for (int i = 0; i < m_vertexCount; i++) + { + result.vertices[j] = vertices[j] * x; + j++; + result.vertices[j] = vertices[j] * y; + j++; + result.vertices[j] = vertices[j] * z; + j++; + } + + result.indexes = new int[indexes.Length]; + indexes.CopyTo(result.indexes,0); + + result.pinMemory(); + + return result; } public Mesh Clone() { Mesh result = new Mesh(); - foreach (Triangle t in m_triangles) + if (m_bdata != null) { - result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); + result.m_bdata = new MeshBuildingData(); + foreach (Triangle t in m_bdata.m_triangles) + { + result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); + } + result.m_bdata.m_centroid = m_bdata.m_centroid; + result.m_bdata.m_centroidDiv = m_bdata.m_centroidDiv; + result.m_bdata.m_obbXmin = m_bdata.m_obbXmin; + result.m_bdata.m_obbXmax = m_bdata.m_obbXmax; + result.m_bdata.m_obbYmin = m_bdata.m_obbYmin; + result.m_bdata.m_obbYmax = m_bdata.m_obbYmax; + result.m_bdata.m_obbZmin = m_bdata.m_obbZmin; + result.m_bdata.m_obbZmax = m_bdata.m_obbZmax; } - result.m_centroid = m_centroid; - result.m_centroidDiv = m_centroidDiv; - result.m_obbXmin = m_obbXmin; - result.m_obbXmax = m_obbXmax; - result.m_obbYmin = m_obbYmin; - result.m_obbYmax = m_obbYmax; - result.m_obbZmin = m_obbZmin; - result.m_obbZmax = m_obbZmax; + result.m_obb = m_obb; + result.m_obboffset = m_obboffset; return result; } @@ -124,37 +183,34 @@ namespace OpenSim.Region.Physics.Meshing float y = v.Y; float z = v.Z; - m_centroid.X += x; - m_centroid.Y += y; - m_centroid.Z += z; - m_centroidDiv++; + m_bdata.m_centroid.X += x; + m_bdata.m_centroid.Y += y; + m_bdata.m_centroid.Z += z; + m_bdata.m_centroidDiv++; - if (x > m_obbXmax) - m_obbXmax = x; - else if (x < m_obbXmin) - m_obbXmin = x; + if (x > m_bdata.m_obbXmax) + m_bdata.m_obbXmax = x; + else if (x < m_bdata.m_obbXmin) + m_bdata.m_obbXmin = x; - if (y > m_obbYmax) - m_obbYmax = y; - else if (y < m_obbYmin) - m_obbYmin = y; + if (y > m_bdata.m_obbYmax) + m_bdata.m_obbYmax = y; + else if (y < m_bdata.m_obbYmin) + m_bdata.m_obbYmin = y; - if (z > m_obbZmax) - m_obbZmax = z; - else if (z < m_obbZmin) - m_obbZmin = z; + if (z > m_bdata.m_obbZmax) + m_bdata.m_obbZmax = z; + else if (z < m_bdata.m_obbZmin) + m_bdata.m_obbZmin = z; } public void Add(Triangle triangle) { - if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) throw new NotSupportedException("Attempt to Add to a pinned Mesh"); - // If a vertex of the triangle is not yet in the vertices list, - // add it and set its index to the current index count - // vertex == seems broken - // skip colapsed triangles + if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z) @@ -163,46 +219,45 @@ namespace OpenSim.Region.Physics.Meshing return; } - if (m_vertices.Count == 0) + if (m_bdata.m_vertices.Count == 0) { - m_centroidDiv = 0; - m_centroid = Vector3.Zero; + m_bdata.m_centroidDiv = 0; + m_bdata.m_centroid = Vector3.Zero; } - if (!m_vertices.ContainsKey(triangle.v1)) + if (!m_bdata.m_vertices.ContainsKey(triangle.v1)) { - m_vertices[triangle.v1] = m_vertices.Count; + m_bdata.m_vertices[triangle.v1] = m_bdata.m_vertices.Count; addVertexLStats(triangle.v1); } - if (!m_vertices.ContainsKey(triangle.v2)) + if (!m_bdata.m_vertices.ContainsKey(triangle.v2)) { - m_vertices[triangle.v2] = m_vertices.Count; + m_bdata.m_vertices[triangle.v2] = m_bdata.m_vertices.Count; addVertexLStats(triangle.v2); } - if (!m_vertices.ContainsKey(triangle.v3)) + if (!m_bdata.m_vertices.ContainsKey(triangle.v3)) { - m_vertices[triangle.v3] = m_vertices.Count; + m_bdata.m_vertices[triangle.v3] = m_bdata.m_vertices.Count; addVertexLStats(triangle.v3); } - m_triangles.Add(triangle); + m_bdata.m_triangles.Add(triangle); } public Vector3 GetCentroid() { - if (m_centroidDiv > 0) - return new Vector3(m_centroid.X / m_centroidDiv, m_centroid.Y / m_centroidDiv, m_centroid.Z / m_centroidDiv); - else - return Vector3.Zero; + return m_obboffset; + } public Vector3 GetOBB() { + return m_obb; float x, y, z; - if (m_centroidDiv > 0) + if (m_bdata.m_centroidDiv > 0) { - x = (m_obbXmax - m_obbXmin) * 0.5f; - y = (m_obbYmax - m_obbYmin) * 0.5f; - z = (m_obbZmax - m_obbZmin) * 0.5f; + x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f; + y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f; + z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f; } else // ?? { @@ -213,72 +268,10 @@ namespace OpenSim.Region.Physics.Meshing return new Vector3(x, y, z); } - public void CalcNormals() - { - int iTriangles = m_triangles.Count; - - this.m_normals = new float[iTriangles * 3]; - - int i = 0; - foreach (Triangle t in m_triangles) - { - float ux, uy, uz; - float vx, vy, vz; - float wx, wy, wz; - - ux = t.v1.X; - uy = t.v1.Y; - uz = t.v1.Z; - - vx = t.v2.X; - vy = t.v2.Y; - vz = t.v2.Z; - - wx = t.v3.X; - wy = t.v3.Y; - wz = t.v3.Z; - - - // Vectors for edges - float e1x, e1y, e1z; - float e2x, e2y, e2z; - - e1x = ux - vx; - e1y = uy - vy; - e1z = uz - vz; - - e2x = ux - wx; - e2y = uy - wy; - e2z = uz - wz; - - - // Cross product for normal - float nx, ny, nz; - nx = e1y * e2z - e1z * e2y; - ny = e1z * e2x - e1x * e2z; - nz = e1x * e2y - e1y * e2x; - - // Length - float l = (float)Math.Sqrt(nx * nx + ny * ny + nz * nz); - float lReciprocal = 1.0f / l; - - // Normalized "normal" - //nx /= l; - //ny /= l; - //nz /= l; - - m_normals[i] = nx * lReciprocal; - m_normals[i + 1] = ny * lReciprocal; - m_normals[i + 2] = nz * lReciprocal; - - i += 3; - } - } - public List getVertexList() { List result = new List(); - foreach (Vertex v in m_vertices.Keys) + foreach (Vertex v in m_bdata.m_vertices.Keys) { result.Add(new Vector3(v.X, v.Y, v.Z)); } @@ -287,10 +280,10 @@ namespace OpenSim.Region.Physics.Meshing private float[] getVertexListAsFloat() { - if (m_vertices == null) + if (m_bdata.m_vertices == null) throw new NotSupportedException(); - float[] result = new float[m_vertices.Count * 3]; - foreach (KeyValuePair kvp in m_vertices) + float[] result = new float[m_bdata.m_vertices.Count * 3]; + foreach (KeyValuePair kvp in m_bdata.m_vertices) { Vertex v = kvp.Key; int i = kvp.Value; @@ -303,74 +296,39 @@ namespace OpenSim.Region.Physics.Meshing public float[] getVertexListAsFloatLocked() { - if (m_pinnedVertexes.IsAllocated) - return (float[])(m_pinnedVertexes.Target); - - float[] result = getVertexListAsFloat(); - m_pinnedVertexes = GCHandle.Alloc(result, GCHandleType.Pinned); - // Inform the garbage collector of this unmanaged allocation so it can schedule - // the next GC round more intelligently - GC.AddMemoryPressure(Buffer.ByteLength(result)); - - return result; - } - - public void PrepForOde() - { - // If there isn't an unmanaged array allocated yet, do it now - if (m_verticesPtr == IntPtr.Zero) - { - float[] vertexList = getVertexListAsFloat(); - // Each vertex is 3 elements (floats) - m_vertexCount = vertexList.Length / 3; - int byteCount = m_vertexCount * 3 * sizeof(float); - m_verticesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount); - System.Runtime.InteropServices.Marshal.Copy(vertexList, 0, m_verticesPtr, m_vertexCount * 3); - } - - // If there isn't an unmanaged array allocated yet, do it now - if (m_indicesPtr == IntPtr.Zero) - { - int[] indexList = getIndexListAsInt(); - m_indexCount = indexList.Length; - int byteCount = m_indexCount * sizeof(int); - m_indicesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount); - System.Runtime.InteropServices.Marshal.Copy(indexList, 0, m_indicesPtr, m_indexCount); - } - - releaseSourceMeshData(); + return null; } - public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount) + public void getVertexListAsPtrToFloatArray(out IntPtr _vertices, out int vertexStride, out int vertexCount) { // A vertex is 3 floats vertexStride = 3 * sizeof(float); // If there isn't an unmanaged array allocated yet, do it now - if (m_verticesPtr == IntPtr.Zero) + if (m_verticesPtr == IntPtr.Zero && m_bdata != null) { - float[] vertexList = getVertexListAsFloat(); + vertices = getVertexListAsFloat(); // Each vertex is 3 elements (floats) - m_vertexCount = vertexList.Length / 3; - int byteCount = m_vertexCount * vertexStride; - m_verticesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount); - System.Runtime.InteropServices.Marshal.Copy(vertexList, 0, m_verticesPtr, m_vertexCount * 3); + m_vertexCount = vertices.Length / 3; + vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned); + m_verticesPtr = vhandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(vertices)); } - vertices = m_verticesPtr; + _vertices = m_verticesPtr; vertexCount = m_vertexCount; } public int[] getIndexListAsInt() { - if (m_triangles == null) + if (m_bdata.m_triangles == null) throw new NotSupportedException(); - int[] result = new int[m_triangles.Count * 3]; - for (int i = 0; i < m_triangles.Count; i++) + int[] result = new int[m_bdata.m_triangles.Count * 3]; + for (int i = 0; i < m_bdata.m_triangles.Count; i++) { - Triangle t = m_triangles[i]; - result[3 * i + 0] = m_vertices[t.v1]; - result[3 * i + 1] = m_vertices[t.v2]; - result[3 * i + 2] = m_vertices[t.v3]; + Triangle t = m_bdata.m_triangles[i]; + result[3 * i + 0] = m_bdata.m_vertices[t.v1]; + result[3 * i + 1] = m_bdata.m_vertices[t.v2]; + result[3 * i + 2] = m_bdata.m_vertices[t.v3]; } return result; } @@ -381,28 +339,19 @@ namespace OpenSim.Region.Physics.Meshing /// public int[] getIndexListAsIntLocked() { - if (m_pinnedIndex.IsAllocated) - return (int[])(m_pinnedIndex.Target); - - int[] result = getIndexListAsInt(); - m_pinnedIndex = GCHandle.Alloc(result, GCHandleType.Pinned); - // Inform the garbage collector of this unmanaged allocation so it can schedule - // the next GC round more intelligently - GC.AddMemoryPressure(Buffer.ByteLength(result)); - - return result; + return null; } public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount) { // If there isn't an unmanaged array allocated yet, do it now - if (m_indicesPtr == IntPtr.Zero) + if (m_indicesPtr == IntPtr.Zero && m_bdata != null) { - int[] indexList = getIndexListAsInt(); - m_indexCount = indexList.Length; - int byteCount = m_indexCount * sizeof(int); - m_indicesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount); - System.Runtime.InteropServices.Marshal.Copy(indexList, 0, m_indicesPtr, m_indexCount); + indexes = getIndexListAsInt(); + m_indexCount = indexes.Length; + ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned); + m_indicesPtr = ihandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(indexes)); } // A triangle is 3 ints (indices) triStride = 3 * sizeof(int); @@ -412,18 +361,16 @@ namespace OpenSim.Region.Physics.Meshing public void releasePinned() { - if (m_pinnedVertexes.IsAllocated) - m_pinnedVertexes.Free(); - if (m_pinnedIndex.IsAllocated) - m_pinnedIndex.Free(); if (m_verticesPtr != IntPtr.Zero) { - System.Runtime.InteropServices.Marshal.FreeHGlobal(m_verticesPtr); + vhandler.Free(); + vertices = null; m_verticesPtr = IntPtr.Zero; } if (m_indicesPtr != IntPtr.Zero) { - System.Runtime.InteropServices.Marshal.FreeHGlobal(m_indicesPtr); + ihandler.Free(); + indexes = null; m_indicesPtr = IntPtr.Zero; } } @@ -433,29 +380,42 @@ namespace OpenSim.Region.Physics.Meshing /// public void releaseSourceMeshData() { - m_triangles = null; - m_vertices = null; + if (m_bdata != null) + { + m_bdata.m_triangles = null; + m_bdata.m_vertices = null; + } + } + + public void releaseBuildingMeshData() + { + if (m_bdata != null) + { + m_bdata.m_triangles = null; + m_bdata.m_vertices = null; + m_bdata = null; + } } public void Append(IMesh newMesh) { - if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) throw new NotSupportedException("Attempt to Append to a pinned Mesh"); if (!(newMesh is Mesh)) return; - foreach (Triangle t in ((Mesh)newMesh).m_triangles) + foreach (Triangle t in ((Mesh)newMesh).m_bdata.m_triangles) Add(t); } // Do a linear transformation of mesh. public void TransformLinear(float[,] matrix, float[] offset) { - if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh"); - - foreach (Vertex v in m_vertices.Keys) + + foreach (Vertex v in m_bdata.m_vertices.Keys) { if (v == null) continue; @@ -473,10 +433,12 @@ namespace OpenSim.Region.Physics.Meshing { if (path == null) return; + if (m_bdata == null) + return; String fileName = name + "_" + title + ".raw"; String completePath = System.IO.Path.Combine(path, fileName); StreamWriter sw = new StreamWriter(completePath); - foreach (Triangle t in m_triangles) + foreach (Triangle t in m_bdata.m_triangles) { String s = t.ToStringRaw(); sw.WriteLine(s); @@ -486,7 +448,144 @@ namespace OpenSim.Region.Physics.Meshing public void TrimExcess() { - m_triangles.TrimExcess(); + m_bdata.m_triangles.TrimExcess(); + } + + public void pinMemory() + { + m_vertexCount = vertices.Length / 3; + vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned); + m_verticesPtr = vhandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(vertices)); + + m_indexCount = indexes.Length; + ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned); + m_indicesPtr = ihandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(indexes)); + } + + public void PrepForOde() + { + // If there isn't an unmanaged array allocated yet, do it now + if (m_verticesPtr == IntPtr.Zero) + vertices = getVertexListAsFloat(); + + // If there isn't an unmanaged array allocated yet, do it now + if (m_indicesPtr == IntPtr.Zero) + indexes = getIndexListAsInt(); + + pinMemory(); + + float x, y, z; + + if (m_bdata.m_centroidDiv > 0) + { + m_obboffset = new Vector3(m_bdata.m_centroid.X / m_bdata.m_centroidDiv, m_bdata.m_centroid.Y / m_bdata.m_centroidDiv, m_bdata.m_centroid.Z / m_bdata.m_centroidDiv); + x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f; + y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f; + z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f; + } + + else + { + m_obboffset = Vector3.Zero; + x = 0.5f; + y = 0.5f; + z = 0.5f; + } + m_obb = new Vector3(x, y, z); + + releaseBuildingMeshData(); + } + public bool ToStream(Stream st) + { + if (m_indicesPtr == IntPtr.Zero || m_verticesPtr == IntPtr.Zero) + return false; + + BinaryWriter bw = new BinaryWriter(st); + bool ok = true; + + try + { + + bw.Write(m_vertexCount); + bw.Write(m_indexCount); + + for (int i = 0; i < 3 * m_vertexCount; i++) + bw.Write(vertices[i]); + for (int i = 0; i < m_indexCount; i++) + bw.Write(indexes[i]); + bw.Write(m_obb.X); + bw.Write(m_obb.Y); + bw.Write(m_obb.Z); + bw.Write(m_obboffset.X); + bw.Write(m_obboffset.Y); + bw.Write(m_obboffset.Z); + } + catch + { + ok = false; + } + + if (bw != null) + { + bw.Flush(); + bw.Close(); + } + + return ok; + } + + public static Mesh FromStream(Stream st, AMeshKey key) + { + Mesh mesh = new Mesh(); + mesh.releaseBuildingMeshData(); + + BinaryReader br = new BinaryReader(st); + + bool ok = true; + try + { + mesh.m_vertexCount = br.ReadInt32(); + mesh.m_indexCount = br.ReadInt32(); + + int n = 3 * mesh.m_vertexCount; + mesh.vertices = new float[n]; + for (int i = 0; i < n; i++) + mesh.vertices[i] = br.ReadSingle(); + + mesh.indexes = new int[mesh.m_indexCount]; + for (int i = 0; i < mesh.m_indexCount; i++) + mesh.indexes[i] = br.ReadInt32(); + + mesh.m_obb.X = br.ReadSingle(); + mesh.m_obb.Y = br.ReadSingle(); + mesh.m_obb.Z = br.ReadSingle(); + + mesh.m_obboffset.X = br.ReadSingle(); + mesh.m_obboffset.Y = br.ReadSingle(); + mesh.m_obboffset.Z = br.ReadSingle(); + } + catch + { + ok = false; + } + + br.Close(); + + if (ok) + { + mesh.pinMemory(); + + mesh.Key = key; + mesh.RefCount = 1; + + return mesh; + } + + mesh.vertices = null; + mesh.indexes = null; + return null; } } } diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index dec5eb7..952ecc8 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -42,6 +42,8 @@ using System.Reflection; using System.IO; using ComponentAce.Compression.Libs.zlib; using OpenSim.Region.Physics.ConvexDecompositionDotNet; +using System.Runtime.Serialization; +using System.Runtime.Serialization.Formatters.Binary; namespace OpenSim.Region.Physics.Meshing { @@ -68,18 +70,20 @@ namespace OpenSim.Region.Physics.Meshing // Setting baseDir to a path will enable the dumping of raw files // raw files can be imported by blender so a visual inspection of the results can be done -#if SPAM - const string baseDir = "rawFiles"; -#else + + public object diskLock = new object(); + + public bool doMeshFileCache = true; + + public string cachePath = "MeshCache"; + +// const string baseDir = "rawFiles"; private const string baseDir = null; //"rawFiles"; -#endif private bool useMeshiesPhysicsMesh = false; private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh -// private Dictionary m_uniqueMeshes = new Dictionary(); -// private Dictionary m_uniqueReleasedMeshes = new Dictionary(); private Dictionary m_uniqueMeshes = new Dictionary(); private Dictionary m_uniqueReleasedMeshes = new Dictionary(); @@ -89,8 +93,16 @@ namespace OpenSim.Region.Physics.Meshing IConfig mesh_config = config.Configs["Mesh"]; if(mesh_config != null) + { useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); - + if(useMeshiesPhysicsMesh) + { + doMeshFileCache = mesh_config.GetBoolean("MeshFileCache", doMeshFileCache); + cachePath = mesh_config.GetString("MeshFileCachePath", cachePath); + } + else + doMeshFileCache = false; + } } /// @@ -188,7 +200,7 @@ namespace OpenSim.Region.Physics.Meshing /// Size of entire object /// /// - private void AddSubMesh(OSDMap subMeshData, Vector3 size, List coords, List faces) + private void AddSubMesh(OSDMap subMeshData, List coords, List faces) { // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap)); @@ -221,9 +233,9 @@ namespace OpenSim.Region.Physics.Meshing ushort uZ = Utils.BytesToUInt16(posBytes, i + 4); Coord c = new Coord( - Utils.UInt16ToFloat(uX, posMin.X, posMax.X) * size.X, - Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y) * size.Y, - Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z) * size.Z); + Utils.UInt16ToFloat(uX, posMin.X, posMax.X), + Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y), + Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z)); coords.Add(c); } @@ -247,7 +259,7 @@ namespace OpenSim.Region.Physics.Meshing /// /// /// - private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool convex) + private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, float lod, bool convex) { // m_log.DebugFormat( // "[MESH]: Creating physics proxy for {0}, shape {1}", @@ -263,18 +275,18 @@ namespace OpenSim.Region.Physics.Meshing if (!useMeshiesPhysicsMesh) return null; - if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, size, out coords, out faces, convex)) + if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, out coords, out faces, convex)) return null; } else { - if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) + if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, lod, out coords, out faces)) return null; } } else { - if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) + if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, lod, out coords, out faces)) return null; } @@ -309,7 +321,7 @@ namespace OpenSim.Region.Physics.Meshing /// Faces are added to this list by the method. /// true if coords and faces were successfully generated, false if not private bool GenerateCoordsAndFacesFromPrimMeshData( - string primName, PrimitiveBaseShape primShape, Vector3 size, out List coords, out List faces, bool convex) + string primName, PrimitiveBaseShape primShape, out List coords, out List faces, bool convex) { // m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); @@ -382,7 +394,7 @@ namespace OpenSim.Region.Physics.Meshing OSD decodedMeshOsd = new OSD(); byte[] meshBytes = new byte[physSize]; System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize); -// byte[] decompressed = new byte[physSize * 5]; + try { using (MemoryStream inMs = new MemoryStream(meshBytes)) @@ -420,13 +432,13 @@ namespace OpenSim.Region.Physics.Meshing // physics_shape is an array of OSDMaps, one for each submesh if (decodedMeshOsd is OSDArray) { -// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); +// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); decodedMeshOsdArray = (OSDArray)decodedMeshOsd; foreach (OSD subMeshOsd in decodedMeshOsdArray) { if (subMeshOsd is OSDMap) - AddSubMesh(subMeshOsd as OSDMap, size, coords, faces); + AddSubMesh(subMeshOsd as OSDMap, coords, faces); } } } @@ -498,9 +510,9 @@ namespace OpenSim.Region.Physics.Meshing t3 = data[ptr++]; t3 += data[ptr++] << 8; - f3 = new float3((t1 * range.X + min.X) * size.X, - (t2 * range.Y + min.Y) * size.Y, - (t3 * range.Z + min.Z) * size.Z); + f3 = new float3((t1 * range.X + min.X), + (t2 * range.Y + min.Y), + (t3 * range.Z + min.Z)); vs.Add(f3); } @@ -597,9 +609,9 @@ namespace OpenSim.Region.Physics.Meshing t3 = data[i++]; t3 += data[i++] << 8; - f3 = new float3((t1 * range.X + min.X) * size.X, - (t2 * range.Y + min.Y) * size.Y, - (t3 * range.Z + min.Z) * size.Z); + f3 = new float3((t1 * range.X + min.X), + (t2 * range.Y + min.Y), + (t3 * range.Z + min.Z)); vs.Add(f3); } @@ -687,7 +699,7 @@ namespace OpenSim.Region.Physics.Meshing /// Faces are added to this list by the method. /// true if coords and faces were successfully generated, false if not private bool GenerateCoordsAndFacesFromPrimSculptData( - string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, out List coords, out List faces) + string primName, PrimitiveBaseShape primShape, float lod, out List coords, out List faces) { coords = new List(); faces = new List(); @@ -757,9 +769,7 @@ namespace OpenSim.Region.Physics.Meshing idata.Dispose(); - sculptMesh.DumpRaw(baseDir, primName, "primMesh"); - - sculptMesh.Scale(size.X, size.Y, size.Z); +// sculptMesh.DumpRaw(baseDir, primName, "primMesh"); coords = sculptMesh.coords; faces = sculptMesh.faces; @@ -777,7 +787,7 @@ namespace OpenSim.Region.Physics.Meshing /// Faces are added to this list by the method. /// true if coords and faces were successfully generated, false if not private bool GenerateCoordsAndFacesFromPrimShapeData( - string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, out List coords, out List faces) + string primName, PrimitiveBaseShape primShape, float lod, out List coords, out List faces) { PrimMesh primMesh; coords = new List(); @@ -912,9 +922,7 @@ namespace OpenSim.Region.Physics.Meshing } } - primMesh.DumpRaw(baseDir, primName, "primMesh"); - - primMesh.Scale(size.X, size.Y, size.Z); +// primMesh.DumpRaw(baseDir, primName, "primMesh"); coords = primMesh.coords; faces = primMesh.faces; @@ -934,6 +942,7 @@ namespace OpenSim.Region.Physics.Meshing { key.uuid = primShape.SculptTexture; key.hashB = mdjb2(key.hashB, primShape.SculptType); + key.hashB = mdjb2(key.hashB, primShape.PCode); } else { @@ -956,6 +965,7 @@ namespace OpenSim.Region.Physics.Meshing hash = mdjb2(hash, primShape.ProfileBegin); hash = mdjb2(hash, primShape.ProfileEnd); hash = mdjb2(hash, primShape.ProfileHollow); + hash = mdjb2(hash, primShape.PCode); key.hashA = hash; } @@ -1001,8 +1011,6 @@ namespace OpenSim.Region.Physics.Meshing return CreateMesh(primName, primShape, size, lod, false,false,false); } - private static Vector3 m_MeshUnitSize = new Vector3(0.5f, 0.5f, 0.5f); - public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) { Mesh mesh = null; @@ -1031,7 +1039,13 @@ namespace OpenSim.Region.Physics.Meshing { m_uniqueReleasedMeshes.Remove(key); lock (m_uniqueMeshes) - m_uniqueMeshes.Add(key, mesh); + { + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + } mesh.RefCount = 1; return mesh; } @@ -1039,6 +1053,8 @@ namespace OpenSim.Region.Physics.Meshing return null; } + private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f); + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) { #if SPAM @@ -1074,41 +1090,78 @@ namespace OpenSim.Region.Physics.Meshing { m_uniqueReleasedMeshes.Remove(key); lock (m_uniqueMeshes) - m_uniqueMeshes.Add(key, mesh); + { + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + } mesh.RefCount = 1; return mesh; } } - mesh = CreateMeshFromPrimMesher(primName, primShape, size, lod,convex); + Mesh UnitMesh = null; + AMeshKey unitKey = GetMeshUniqueKey(primShape, m_MeshUnitSize, (byte)lod, convex); - if (mesh != null) + lock (m_uniqueReleasedMeshes) { - if ((!isPhysical) && size.X < minSizeForComplexMesh && size.Y < minSizeForComplexMesh && size.Z < minSizeForComplexMesh) + m_uniqueReleasedMeshes.TryGetValue(unitKey, out UnitMesh); + if (UnitMesh != null) { -#if SPAM - m_log.Debug("Meshmerizer: prim " + primName + " has a size of " + size.ToString() + " which is below threshold of " + - minSizeForComplexMesh.ToString() + " - creating simple bounding box"); -#endif - mesh = CreateBoundingBoxMesh(mesh); - mesh.DumpRaw(baseDir, primName, "Z extruded"); + UnitMesh.RefCount = 1; } + } + + if (UnitMesh == null && primShape.SculptEntry && doMeshFileCache) + UnitMesh = GetFromFileCache(unitKey); + + if (UnitMesh == null) + { + UnitMesh = CreateMeshFromPrimMesher(primName, primShape, lod, convex); + + if (UnitMesh == null) + return null; + + UnitMesh.DumpRaw(baseDir, unitKey.ToString(), "Z"); if (forOde) { // force pinned mem allocation - mesh.PrepForOde(); + UnitMesh.PrepForOde(); } else - mesh.TrimExcess(); + UnitMesh.TrimExcess(); - mesh.Key = key; - mesh.RefCount = 1; + UnitMesh.Key = unitKey; + UnitMesh.RefCount = 1; - lock(m_uniqueMeshes) - m_uniqueMeshes.Add(key, mesh); + if (doMeshFileCache && primShape.SculptEntry) + StoreToFileCache(unitKey, UnitMesh); + + lock (m_uniqueReleasedMeshes) + { + try + { + m_uniqueReleasedMeshes.Add(unitKey, UnitMesh); + } + catch { } + } } + mesh = UnitMesh.Scale(size); + mesh.Key = key; + mesh.RefCount = 1; + lock (m_uniqueMeshes) + { + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + } + return mesh; } @@ -1133,7 +1186,13 @@ namespace OpenSim.Region.Physics.Meshing mesh.RefCount = 0; m_uniqueMeshes.Remove(mesh.Key); lock (m_uniqueReleasedMeshes) - m_uniqueReleasedMeshes.Add(mesh.Key, mesh); + { + try + { + m_uniqueReleasedMeshes.Add(mesh.Key, mesh); + } + catch { } + } } } @@ -1160,10 +1219,102 @@ namespace OpenSim.Region.Physics.Meshing foreach (Mesh m in meshstodelete) { m_uniqueReleasedMeshes.Remove(m.Key); - m.releaseSourceMeshData(); + m.releaseBuildingMeshData(); m.releasePinned(); } } } + + public void FileNames(AMeshKey key, out string dir,out string fullFileName) + { + string id = key.ToString(); + string init = id.Substring(0, 1); + dir = System.IO.Path.Combine(cachePath, init); + fullFileName = System.IO.Path.Combine(dir, id); + } + + public string FullFileName(AMeshKey key) + { + string id = key.ToString(); + string init = id.Substring(0,1); + id = System.IO.Path.Combine(init, id); + id = System.IO.Path.Combine(cachePath, id); + return id; + } + + private Mesh GetFromFileCache(AMeshKey key) + { + Mesh mesh = null; + string filename = FullFileName(key); + bool ok = true; + + lock (diskLock) + { + if (File.Exists(filename)) + { + FileStream stream = null; + try + { + stream = File.Open(filename, FileMode.Open, FileAccess.Read, FileShare.Read); + BinaryFormatter bformatter = new BinaryFormatter(); + + mesh = Mesh.FromStream(stream, key); + } + catch (Exception e) + { + ok = false; + m_log.ErrorFormat( + "[MESH CACHE]: Failed to get file {0}. Exception {1} {2}", + filename, e.Message, e.StackTrace); + } + if (stream != null) + stream.Close(); + + if (mesh == null || !ok) + File.Delete(filename); + } + } + + return mesh; + } + + private void StoreToFileCache(AMeshKey key, Mesh mesh) + { + Stream stream = null; + bool ok = false; + + // Make sure the target cache directory exists + string dir = String.Empty; + string filename = String.Empty; + + FileNames(key, out dir, out filename); + + lock (diskLock) + { + try + { + if (!Directory.Exists(dir)) + { + Directory.CreateDirectory(dir); + } + + stream = File.Open(filename, FileMode.Create); + ok = mesh.ToStream(stream); + } + catch (IOException e) + { + m_log.ErrorFormat( + "[MESH CACHE]: Failed to write file {0}. Exception {1} {2}.", + filename, e.Message, e.StackTrace); + ok = false; + } + + if (stream != null) + stream.Close(); + + if (!ok && File.Exists(filename)) + File.Delete(filename); + } + } } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 03048a4..6592e6c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -718,7 +718,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; - +/* // debug PhysicsActor dp2; if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) @@ -742,7 +742,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } // - +*/ if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || -- cgit v1.1 From 13cb64a2c57db34bcb6706075d3d7998bb416392 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 12 Oct 2012 23:46:48 +0100 Subject: missing file --- OpenSim/Region/Physics/Manager/IMesher.cs | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index fdba6ee..d7ab20b 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -58,6 +58,7 @@ namespace OpenSim.Region.Physics.Manager { } + [Serializable()] [StructLayout(LayoutKind.Explicit)] public struct AMeshKey { @@ -67,6 +68,11 @@ namespace OpenSim.Region.Physics.Manager public ulong hashA; [FieldOffset(8)] public ulong hashB; + + public override string ToString() + { + return uuid.ToString(); + } } public interface IMesh -- cgit v1.1 From 48d2258f41b3ce8e9a1bfe98e9835f5ea947a815 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 13 Oct 2012 00:41:19 +0100 Subject: longer meshs identification keys, so first part on disk cache is it's asset id --- OpenSim/Region/Physics/Manager/IMesher.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index d7ab20b..d0e3996 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -68,10 +68,12 @@ namespace OpenSim.Region.Physics.Manager public ulong hashA; [FieldOffset(8)] public ulong hashB; + [FieldOffset(16)] + public ulong hashC; public override string ToString() { - return uuid.ToString(); + return uuid.ToString() + "-" + hashC.ToString() ; } } -- cgit v1.1 From 9ada03bcdd8bff22fea162672b5a3dde88d4d0c5 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 13 Oct 2012 00:49:08 +0100 Subject: missing file (again) --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 952ecc8..2fe34e3 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -936,13 +936,14 @@ namespace OpenSim.Region.Physics.Meshing Byte[] someBytes; key.hashB = 5181; + key.hashC = 5181; ulong hash = 5381; if (primShape.SculptEntry) { key.uuid = primShape.SculptTexture; - key.hashB = mdjb2(key.hashB, primShape.SculptType); - key.hashB = mdjb2(key.hashB, primShape.PCode); + key.hashC = mdjb2(key.hashC, primShape.SculptType); + key.hashC = mdjb2(key.hashC, primShape.PCode); } else { @@ -954,6 +955,9 @@ namespace OpenSim.Region.Physics.Meshing hash = mdjb2(hash, primShape.PathScaleX); hash = mdjb2(hash, primShape.PathScaleY); hash = mdjb2(hash, primShape.PathShearX); + key.hashA = hash; + key.hashA |= 0xf000000000000000; + hash = key.hashB; hash = mdjb2(hash, primShape.PathShearY); hash = mdjb2(hash, (byte)primShape.PathTwist); hash = mdjb2(hash, (byte)primShape.PathTwistBegin); @@ -966,10 +970,10 @@ namespace OpenSim.Region.Physics.Meshing hash = mdjb2(hash, primShape.ProfileEnd); hash = mdjb2(hash, primShape.ProfileHollow); hash = mdjb2(hash, primShape.PCode); - key.hashA = hash; + key.hashB = hash; } - hash = key.hashB; + hash = key.hashC; someBytes = size.GetBytes(); for (int i = 0; i < someBytes.Length; i++) -- cgit v1.1 From 666fb744a36ca5497dc8868298f47c23cd72b1a8 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 13 Oct 2012 01:41:18 +0100 Subject: retouch mesh ids --- OpenSim/Region/Physics/Manager/IMesher.cs | 2 +- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 30 +++++++++++++++-------- 2 files changed, 21 insertions(+), 11 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index d0e3996..21a5fa0 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -73,7 +73,7 @@ namespace OpenSim.Region.Physics.Manager public override string ToString() { - return uuid.ToString() + "-" + hashC.ToString() ; + return uuid.ToString() + "-" + hashC.ToString("x") ; } } diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 2fe34e3..35eabd4 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -956,7 +956,6 @@ namespace OpenSim.Region.Physics.Meshing hash = mdjb2(hash, primShape.PathScaleY); hash = mdjb2(hash, primShape.PathShearX); key.hashA = hash; - key.hashA |= 0xf000000000000000; hash = key.hashB; hash = mdjb2(hash, primShape.PathShearY); hash = mdjb2(hash, (byte)primShape.PathTwist); @@ -975,21 +974,32 @@ namespace OpenSim.Region.Physics.Meshing hash = key.hashC; - someBytes = size.GetBytes(); - for (int i = 0; i < someBytes.Length; i++) - hash = mdjb2(hash, someBytes[i]); - hash = mdjb2(hash, lod); - - hash &= 0x3fffffffffffffff; + + if (size == m_MeshUnitSize) + { + hash = hash << 8; + hash |= 8; + } + else + { + someBytes = size.GetBytes(); + for (int i = 0; i < someBytes.Length; i++) + hash = mdjb2(hash, someBytes[i]); + hash = hash << 8; + } if (convex) - hash |= 0x4000000000000000; + hash |= 4; if (primShape.SculptEntry) - hash |= 0x8000000000000000; + { + hash |= 1; + if (primShape.SculptType == (byte)SculptType.Mesh) + hash |= 2; + } - key.hashB = hash; + key.hashC = hash; return key; } -- cgit v1.1 From 5986b4ee3980d27d6e8524c4b8f5ad4c9a701288 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 13 Oct 2012 22:30:34 +0100 Subject: add mesh cache expire on region startup. Expires will be relative to previus expire (assumed done only once at startup). File 'cntr' on cache folder stores time. Deleting it will force a skip on expire. Default time is 48hours before previus startup to account for failed ones etc. --- OpenSim/Region/Physics/Manager/IMesher.cs | 1 + OpenSim/Region/Physics/Manager/ZeroMesher.cs | 1 + OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 1 + OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 88 ++++++++++++++++++++++- 4 files changed, 88 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index 21a5fa0..ecc2918 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -41,6 +41,7 @@ namespace OpenSim.Region.Physics.Manager IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex); void ReleaseMesh(IMesh mesh); void ExpireReleaseMeshs(); + void ExpireFileCache(); } // Values for level of detail to be passed to the mesher. diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs index 1411165..16846e6 100644 --- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs +++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs @@ -87,5 +87,6 @@ namespace OpenSim.Region.Physics.Manager public void ReleaseMesh(IMesh mesh) { } public void ExpireReleaseMeshs() { } + public void ExpireFileCache() { } } } diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index fd4ac7f..f629c4d 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -755,5 +755,6 @@ namespace OpenSim.Region.Physics.Meshing public void ReleaseMesh(IMesh imesh) { } public void ExpireReleaseMeshs() { } + public void ExpireFileCache() { } } } diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 35eabd4..6550b66 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -76,6 +76,8 @@ namespace OpenSim.Region.Physics.Meshing public bool doMeshFileCache = true; public string cachePath = "MeshCache"; + public TimeSpan CacheExpire; + public bool doCacheExpire = true; // const string baseDir = "rawFiles"; private const string baseDir = null; //"rawFiles"; @@ -92,17 +94,29 @@ namespace OpenSim.Region.Physics.Meshing IConfig start_config = config.Configs["Startup"]; IConfig mesh_config = config.Configs["Mesh"]; + + float fcache = 48.0f; +// float fcache = 0.02f; + if(mesh_config != null) { useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); - if(useMeshiesPhysicsMesh) + if (useMeshiesPhysicsMesh) { doMeshFileCache = mesh_config.GetBoolean("MeshFileCache", doMeshFileCache); cachePath = mesh_config.GetString("MeshFileCachePath", cachePath); + fcache = mesh_config.GetFloat("MeshFileCacheExpireHours", fcache); + doCacheExpire = mesh_config.GetBoolean("MeshFileCacheDoExpire", doCacheExpire); } else + { doMeshFileCache = false; + doCacheExpire = false; + } } + + CacheExpire = TimeSpan.FromHours(fcache); + } /// @@ -1273,6 +1287,7 @@ namespace OpenSim.Region.Physics.Meshing BinaryFormatter bformatter = new BinaryFormatter(); mesh = Mesh.FromStream(stream, key); + } catch (Exception e) { @@ -1281,11 +1296,14 @@ namespace OpenSim.Region.Physics.Meshing "[MESH CACHE]: Failed to get file {0}. Exception {1} {2}", filename, e.Message, e.StackTrace); } + if (stream != null) stream.Close(); if (mesh == null || !ok) File.Delete(filename); + else + File.SetLastAccessTimeUtc(filename, DateTime.UtcNow); } } @@ -1326,8 +1344,72 @@ namespace OpenSim.Region.Physics.Meshing if (stream != null) stream.Close(); - if (!ok && File.Exists(filename)) - File.Delete(filename); + if (File.Exists(filename)) + { + if (ok) + File.SetLastAccessTimeUtc(filename, DateTime.UtcNow); + else + File.Delete(filename); + } + } + } + + public void ExpireFileCache() + { + if (!doCacheExpire) + return; + + string controlfile = System.IO.Path.Combine(cachePath, "cntr"); + + lock (diskLock) + { + try + { + if (File.Exists(controlfile)) + { + int ndeleted = 0; + int totalfiles = 0; + int ndirs = 0; + DateTime OlderTime = File.GetLastAccessTimeUtc(controlfile) - CacheExpire; + File.SetLastAccessTimeUtc(controlfile, DateTime.UtcNow); + + foreach (string dir in Directory.GetDirectories(cachePath)) + { + try + { + foreach (string file in Directory.GetFiles(dir)) + { + try + { + if (File.GetLastAccessTimeUtc(file) < OlderTime) + { + File.Delete(file); + ndeleted++; + } + } + catch { } + totalfiles++; + } + } + catch { } + ndirs++; + } + + if (ndeleted == 0) + m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, no expires", + totalfiles,ndirs); + else + m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, expired {2} files accessed before {3}", + totalfiles,ndirs, ndeleted, OlderTime.ToString()); + } + else + { + m_log.Info("[MESH CACHE]: Expire delayed to next startup"); + FileStream fs = File.Create(controlfile,4096,FileOptions.WriteThrough); + fs.Close(); + } + } + catch { } } } } -- cgit v1.1 From 1e0334441138cb6fcb8853344310c6dbfc9ac047 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 13 Oct 2012 22:45:09 +0100 Subject: missing file --- OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 2 ++ 1 file changed, 2 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 24f76d62..768b0e6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -103,6 +103,8 @@ namespace OpenSim.Region.Physics.OdePlugin private void DoWork() { + m_mesher.ExpireFileCache(); + while(m_running) { ODEPhysRepData nextRep = createqueue.Dequeue(); -- cgit v1.1 From 91b83fd45e8648febc8176aa50110ff60ece167c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 16 Oct 2012 11:26:05 +0100 Subject: fixes --- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 87 ++++++++++++---------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 9 ++- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 53 ++++++------- 3 files changed, 82 insertions(+), 67 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 768b0e6..73dd2fd 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -32,7 +32,9 @@ namespace OpenSim.Region.Physics.OdePlugin FailMask = 0xC0, // 11000000 AssetFailed = 0x40, // 01000000 - MeshFailed = 0x80 // 10000000 + MeshFailed = 0x80, // 10000000 + + MeshNoColide = FailMask | needAsset } public enum meshWorkerCmnds : byte @@ -119,13 +121,10 @@ namespace OpenSim.Region.Physics.OdePlugin case meshWorkerCmnds.changefull: case meshWorkerCmnds.changeshapetype: case meshWorkerCmnds.changesize: + GetMesh(nextRep); if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor)) m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep); break; - case meshWorkerCmnds.addnew: - if (CreateActorPhysRep(nextRep)) - m_scene.AddChange(nextRep.actor, changes.AddPhysRep, nextRep); - break; case meshWorkerCmnds.getmesh: DoRepDataGetMesh(nextRep); break; @@ -155,13 +154,13 @@ namespace OpenSim.Region.Physics.OdePlugin repData.size = size; repData.shapetype = shapetype; - CheckMeshDone(repData); + CheckMesh(repData); CalcVolumeData(repData); m_scene.AddChange(actor, changes.PhysRepData, repData); return; } - public void NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, + public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, Vector3 size, byte shapetype) { ODEPhysRepData repData = new ODEPhysRepData(); @@ -170,21 +169,17 @@ namespace OpenSim.Region.Physics.OdePlugin repData.size = size; repData.shapetype = shapetype; - CheckMeshDone(repData); + CheckMesh(repData); CalcVolumeData(repData); m_scene.AddChange(actor, changes.AddPhysRep, repData); + return repData; } public void RequestMesh(ODEPhysRepData repData) { repData.mesh = null; - if (repData.meshState == MeshState.needMesh) - { - repData.comand = meshWorkerCmnds.changefull; - createqueue.Enqueue(repData); - } - else if (repData.meshState == MeshState.needAsset) + if (repData.meshState == MeshState.needAsset) { PrimitiveBaseShape pbs = repData.pbs; @@ -196,9 +191,7 @@ namespace OpenSim.Region.Physics.OdePlugin return; } - if (pbs.SculptTexture != repData.assetID) - return; - + repData.assetID = pbs.SculptTexture; repData.meshState = MeshState.loadingAsset; repData.comand = meshWorkerCmnds.getmesh; @@ -209,7 +202,6 @@ namespace OpenSim.Region.Physics.OdePlugin // creates and prepares a mesh to use and calls parameters estimation public bool CreateActorPhysRep(ODEPhysRepData repData) { - getMesh(repData); IMesh mesh = repData.mesh; if (mesh != null) @@ -270,6 +262,15 @@ namespace OpenSim.Region.Physics.OdePlugin if (repData.assetID != repData.pbs.SculptTexture) return; + // check if it is in cache + GetMesh(repData); + if (repData.meshState != MeshState.needAsset) + { + CreateActorPhysRep(repData); + m_scene.AddChange(repData.actor, changes.PhysRepData, repData); + return; + } + RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; if (assetProvider == null) return; @@ -400,7 +401,7 @@ namespace OpenSim.Region.Physics.OdePlugin // see if we need a mesh and if so if we have a cached one // called with a new repData - public bool CheckMeshDone(ODEPhysRepData repData) + public void CheckMesh(ODEPhysRepData repData) { PhysicsActor actor = repData.actor; PrimitiveBaseShape pbs = repData.pbs; @@ -408,7 +409,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!needsMeshing(pbs)) { repData.meshState = MeshState.noNeed; - return true; + return; } IMesh mesh = null; @@ -437,29 +438,38 @@ namespace OpenSim.Region.Physics.OdePlugin if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero) { repData.assetID = pbs.SculptTexture; - repData.meshState = MeshState.needAsset; + repData.meshState = MeshState.needAsset; } else repData.meshState = MeshState.MeshFailed; + + return; } else + { repData.meshState = MeshState.needMesh; - - return false; + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); + if (mesh == null) + { + repData.meshState = MeshState.MeshFailed; + return; + } + } } + repData.meshState = MeshState.AssetOK; repData.mesh = mesh; + if (pbs.SculptEntry) { - repData.meshState = MeshState.AssetOK; repData.assetID = pbs.SculptTexture; } pbs.SculptData = Utils.EmptyBytes; - return true; + return ; } - public bool getMesh(ODEPhysRepData repData) + public void GetMesh(ODEPhysRepData repData) { PhysicsActor actor = repData.actor; @@ -469,17 +479,20 @@ namespace OpenSim.Region.Physics.OdePlugin repData.hasOBB = false; if (!needsMeshing(pbs)) - return false; + { + repData.meshState = MeshState.noNeed; + return; + } if (repData.meshState == MeshState.MeshFailed) - return false; + return; if (pbs.SculptEntry) { if (repData.meshState == MeshState.AssetFailed) { if (pbs.SculptTexture == repData.assetID) - return true; + return; } } @@ -500,27 +513,23 @@ namespace OpenSim.Region.Physics.OdePlugin clod = (int)LevelOfDetail.Low; } - // check cached - mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); - + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); + if (mesh == null) { if (pbs.SculptEntry) { if (pbs.SculptTexture == UUID.Zero) - return false; + return; repData.assetID = pbs.SculptTexture; - repData.meshState = MeshState.AssetOK; if (pbs.SculptData == null || pbs.SculptData.Length == 0) { repData.meshState = MeshState.needAsset; - return false; + return; } } - - mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex,true); } repData.mesh = mesh; @@ -533,12 +542,12 @@ namespace OpenSim.Region.Physics.OdePlugin else repData.meshState = MeshState.MeshFailed; - return false; + return; } repData.meshState = MeshState.AssetOK; - return true; + return; } private void CalculateBasicPrimVolume(ODEPhysRepData repData) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index f083d38..ce67cc4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1104,7 +1104,12 @@ namespace OpenSim.Region.Physics.OdePlugin m_building = true; // control must set this to false when done - _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); + // get basic mass parameters + ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); + + primVolume = repData.volume; + + UpdatePrimBodyData(); } private void resetCollisionAccounting() @@ -1466,7 +1471,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_NoColide = false; - if ((m_meshState & MeshState.FailMask) != 0) + if ((m_meshState & MeshState.MeshNoColide) != 0) m_NoColide = true; else if(m_mesh != null) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 6592e6c..b98f177 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1729,33 +1729,7 @@ namespace OpenSim.Region.Physics.OdePlugin ODEchangeitem item; - if (ChangesQueue.Count > 0) - { - int ttmpstart = Util.EnvironmentTickCount(); - int ttmp; - while (ChangesQueue.Dequeue(out item)) - { - if (item.actor != null) - { - try - { - if (item.actor is OdeCharacter) - ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); - else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) - RemovePrimThreadLocked((OdePrim)item.actor); - } - catch - { - m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", - item.actor.Name, item.what.ToString()); - } - } - ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); - if (ttmp > 20) - break; - } - } d.WorldSetQuickStepNumIterations(world, curphysiteractions); @@ -1766,6 +1740,33 @@ namespace OpenSim.Region.Physics.OdePlugin // clear pointer/counter to contacts to pass into joints m_global_contactcount = 0; + if (ChangesQueue.Count > 0) + { + int ttmpstart = Util.EnvironmentTickCount(); + int ttmp; + + while (ChangesQueue.Dequeue(out item)) + { + if (item.actor != null) + { + try + { + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); + } + catch + { + m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); + } + } + ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp > 20) + break; + } + } // Move characters lock (_characters) -- cgit v1.1 From e8936366f55c588b02c7aa29e40c478f52f16494 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 16 Oct 2012 11:39:58 +0100 Subject: coment a debug warning --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 6550b66..29fdda4 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -347,7 +347,7 @@ namespace OpenSim.Region.Physics.Meshing if (primShape.SculptData.Length <= 0) { - m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName); +// m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName); return false; } -- cgit v1.1 From 33a67b691e078df21cd442e8ec6401b25a849250 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 19 Oct 2012 00:36:32 +0100 Subject: [UNTESTED] core Ode: stop trying to load a broken asset. Make broken assets behave like phantom by Nebadon request --- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 118 +++++++++++++++++++++++----- 1 file changed, 97 insertions(+), 21 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index b2c2d8a..ad6c4ab 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -341,8 +341,17 @@ namespace OpenSim.Region.Physics.OdePlugin prim_geom = geom; //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_assetFailed) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } _parent_scene.geom_name_map[prim_geom] = Name; _parent_scene.actor_name_map[prim_geom] = this; @@ -405,8 +414,17 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.W = _orientation.W; d.BodySetQuaternion(Body, ref myrot); d.GeomSetBody(prim_geom, Body); - m_collisionCategories |= CollisionCategories.Body; - m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + + if (m_assetFailed) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + } d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); @@ -778,8 +796,16 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionCategories &= ~CollisionCategories.Body; m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_assetFailed) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } d.BodyDestroy(Body); lock (childrenPrim) @@ -803,8 +829,17 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionCategories &= ~CollisionCategories.Body; m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_assetFailed) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } Body = IntPtr.Zero; } @@ -1094,8 +1129,16 @@ Console.WriteLine("ZProcessTaints for " + Name); prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); //Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name); - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + if (prm.m_assetFailed) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + } d.Quaternion quat = new d.Quaternion(); quat.W = prm._orientation.W; @@ -1140,10 +1183,18 @@ Console.WriteLine("ZProcessTaints for " + Name); m_collisionCategories |= CollisionCategories.Body; m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); -//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); -//Console.WriteLine(" Post GeomSetCategoryBits 2"); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_assetFailed) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + //Console.WriteLine(" Post GeomSetCategoryBits 2"); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } d.Quaternion quat2 = new d.Quaternion(); quat2.W = _orientation.W; @@ -1304,8 +1355,16 @@ Console.WriteLine("ZProcessTaints for " + Name); disableBodySoft(); } - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_assetFailed) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } if (IsPhysical) { @@ -1326,8 +1385,16 @@ Console.WriteLine("ZProcessTaints for " + Name); if (m_collidesWater) m_collisionFlags |= CollisionCategories.Water; - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + if (m_assetFailed) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } if (IsPhysical) { @@ -1504,6 +1571,8 @@ Console.WriteLine("CreateGeom:"); // m_log.Debug(m_localID); if (mesh == null) CheckMeshAsset(); + else + m_assetFailed = false; } #if SPAM @@ -2007,6 +2076,8 @@ Console.WriteLine(" JointCreateFixed"); mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); if (mesh == null) CheckMeshAsset(); + else + m_assetFailed = false; } } @@ -2060,9 +2131,12 @@ Console.WriteLine(" JointCreateFixed"); m_collisionFlags &= ~CollisionCategories.Water; } - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); - } + if (m_assetFailed) + d.GeomSetCollideBits(prim_geom, 0); + else + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } /// /// Change prim in response to a shape taint. /// @@ -2110,6 +2184,8 @@ Console.WriteLine(" JointCreateFixed"); mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); if (mesh == null) CheckMeshAsset(); + else + m_assetFailed = false; } CreateGeom(m_targetSpace, mesh); @@ -3278,7 +3354,7 @@ Console.WriteLine(" JointCreateFixed"); _pbs.SculptData = new byte[asset.Data.Length]; asset.Data.CopyTo(_pbs.SculptData, 0); - m_assetFailed = false; +// m_assetFailed = false; m_taintshape = true; _parent_scene.AddPhysicsActorTaint(this); } -- cgit v1.1 From 739782627ac2bd076b37f3ff9188cba3eab01d43 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 19 Oct 2012 01:25:30 +0100 Subject: [UNTESTED] core Ode: let broken mesh physical prims collide with land as the defaul basic box so they don't go off world. --- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index ad6c4ab..2e78de5 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -344,7 +344,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); } else { @@ -418,7 +418,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); } else { @@ -851,6 +851,11 @@ namespace OpenSim.Region.Physics.OdePlugin private static Dictionary m_MeshToTriMeshMap = new Dictionary(); + public int BadAssetColideBits() + { + return (m_isphysical ? (int)CollisionCategories.Land : 0); + } + private void setMesh(OdeScene parent_scene, IMesh mesh) { // m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh); @@ -1132,7 +1137,7 @@ Console.WriteLine("ZProcessTaints for " + Name); if (prm.m_assetFailed) { d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, prm.BadAssetColideBits()); } else { @@ -1186,7 +1191,7 @@ Console.WriteLine("ZProcessTaints for " + Name); if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); } else { @@ -1388,7 +1393,7 @@ Console.WriteLine("ZProcessTaints for " + Name); if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); } else { @@ -2132,7 +2137,7 @@ Console.WriteLine(" JointCreateFixed"); } if (m_assetFailed) - d.GeomSetCollideBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); else d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); -- cgit v1.1 From b51c26ab40579a3e7a21df54b15a72a5c464ae41 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 19 Oct 2012 03:50:00 +0100 Subject: UbitOde stop doing a copy of the asset data using just the reference to it. It costs a lot of cpu and doesn't seem all that usefull. --- OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 73dd2fd..5030cec 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -914,8 +914,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (repData.pbs.SculptTexture != repData.assetID) return; - repData.pbs.SculptData = new byte[asset.Data.Length]; - asset.Data.CopyTo(repData.pbs.SculptData,0); +// repData.pbs.SculptData = new byte[asset.Data.Length]; +// asset.Data.CopyTo(repData.pbs.SculptData,0); + repData.pbs.SculptData = asset.Data; repData.meshState = MeshState.AssetOK; m_worker.AssetLoaded(repData); } -- cgit v1.1 From 3b51cae958df48cd629124e875d7236038f60302 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 20 Oct 2012 17:53:33 +0100 Subject: bug fix --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 ++ OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index ce67cc4..76e42d4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1108,6 +1108,8 @@ namespace OpenSim.Region.Physics.OdePlugin ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); primVolume = repData.volume; + m_OBB = repData.OBB; + m_OBBOffset = repData.OBBOffset; UpdatePrimBodyData(); } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index b98f177..cce8946 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2344,7 +2344,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } } - IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + IntPtr HeightmapData = d.GeomUbitTerrainDataCreate(); const int wrap = 0; float thickness = hfmin; -- cgit v1.1 From 8bb29054165b1fa3284b200ca6682eb1c81e977f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 28 Oct 2012 11:55:11 +0000 Subject: test limit ode exec time by time not number of loops done --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index cce8946..5e01ff1 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1714,9 +1714,8 @@ namespace OpenSim.Region.Physics.OdePlugin else curphysiteractions = m_physicsiterations; - int nodeframes = 0; - // checkThread(); + int nodeframes = 0; lock (SimulationLock) lock(OdeLock) @@ -1733,7 +1732,11 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldSetQuickStepNumIterations(world, curphysiteractions); - while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever + int loopstartMS = Util.EnvironmentTickCount(); + int looptimeMS = 0; + + + while (step_time > HalfOdeStep) { try { @@ -1742,9 +1745,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (ChangesQueue.Count > 0) { - int ttmpstart = Util.EnvironmentTickCount(); + int changestartMS = Util.EnvironmentTickCount(); int ttmp; - while (ChangesQueue.Dequeue(out item)) { if (item.actor != null) @@ -1762,7 +1764,7 @@ namespace OpenSim.Region.Physics.OdePlugin item.actor.Name, item.what.ToString()); } } - ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); + ttmp = Util.EnvironmentTickCountSubtract(changestartMS); if (ttmp > 20) break; } @@ -1873,9 +1875,12 @@ namespace OpenSim.Region.Physics.OdePlugin // ode.dunlock(world); } - step_time -= ODE_STEPSIZE; nodeframes++; + + looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS); + if (looptimeMS > 100) + break; } lock (_badCharacter) @@ -1963,7 +1968,7 @@ namespace OpenSim.Region.Physics.OdePlugin // think time dilation as to do with dinamic step size that we dont' have // even so tell something to world - if (nodeframes < 10) // we did the requested loops + if (looptimeMS < 100) // we did the requested loops m_timeDilation = 1.0f; else if (step_time > 0) { -- cgit v1.1 From dd0323d89e9da7121ac85f8997c6f860d9977dd1 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 28 Oct 2012 14:04:39 +0000 Subject: minor change --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 1 + 1 file changed, 1 insertion(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 5e01ff1..0194c21 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1977,6 +1977,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_timeDilation = 1; if (step_time > m_SkipFramesAtms) step_time = 0; + m_lastframe = DateTime.UtcNow; // skip also the time lost } } -- cgit v1.1 From fbe4ec8ee0b56ca4490f5a7439aa287ff37e2e9d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 28 Oct 2012 14:32:55 +0000 Subject: fix ode timing --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 0194c21..eb0a514 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1692,7 +1692,6 @@ namespace OpenSim.Region.Physics.OdePlugin DateTime now = DateTime.UtcNow; TimeSpan timedif = now - m_lastframe; - m_lastframe = now; timeStep = (float)timedif.TotalSeconds; // acumulate time so we can reduce error @@ -1704,6 +1703,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (framecount < 0) framecount = 0; + m_lastframe = now; + framecount++; int curphysiteractions; -- cgit v1.1 From db7f4074b51f6c8e6e329cd67fb1e711f2272408 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 28 Oct 2012 14:39:04 +0000 Subject: revert last change --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index eb0a514..54bc29f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1689,10 +1689,10 @@ namespace OpenSim.Region.Physics.OdePlugin /// public override float Simulate(float timeStep) { - DateTime now = DateTime.UtcNow; TimeSpan timedif = now - m_lastframe; timeStep = (float)timedif.TotalSeconds; + m_lastframe = now; // acumulate time so we can reduce error step_time += timeStep; @@ -1703,7 +1703,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (framecount < 0) framecount = 0; - m_lastframe = now; framecount++; -- cgit v1.1 From c2639bef8e981e49fef09dfd9c54442ba500e9d4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 2 Nov 2012 10:14:08 +0000 Subject: lock unique and uniqueReleased in same order when both locks are needed --- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 34 ++++++++++------------- 1 file changed, 14 insertions(+), 20 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 29fdda4..6e1a105 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -1057,25 +1057,22 @@ namespace OpenSim.Region.Physics.Meshing mesh.RefCount++; return mesh; } - } - // try to find a identical mesh on meshs recently released - lock (m_uniqueReleasedMeshes) - { - m_uniqueReleasedMeshes.TryGetValue(key, out mesh); - if (mesh != null) + // try to find a identical mesh on meshs recently released + lock (m_uniqueReleasedMeshes) { - m_uniqueReleasedMeshes.Remove(key); - lock (m_uniqueMeshes) + m_uniqueReleasedMeshes.TryGetValue(key, out mesh); + if (mesh != null) { + m_uniqueReleasedMeshes.Remove(key); try { m_uniqueMeshes.Add(key, mesh); } catch { } + mesh.RefCount = 1; + return mesh; } - mesh.RefCount = 1; - return mesh; } } return null; @@ -1108,25 +1105,22 @@ namespace OpenSim.Region.Physics.Meshing mesh.RefCount++; return mesh; } - } - // try to find a identical mesh on meshs recently released - lock (m_uniqueReleasedMeshes) - { - m_uniqueReleasedMeshes.TryGetValue(key, out mesh); - if (mesh != null) + // try to find a identical mesh on meshs recently released + lock (m_uniqueReleasedMeshes) { - m_uniqueReleasedMeshes.Remove(key); - lock (m_uniqueMeshes) + m_uniqueReleasedMeshes.TryGetValue(key, out mesh); + if (mesh != null) { + m_uniqueReleasedMeshes.Remove(key); try { m_uniqueMeshes.Add(key, mesh); } catch { } + mesh.RefCount = 1; + return mesh; } - mesh.RefCount = 1; - return mesh; } } -- cgit v1.1 From e36a700eb0732472ca8675bcc520b20674d9037d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 2 Nov 2012 14:02:57 +0000 Subject: add debug position on bad primmesh error --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 76e42d4..dc247a9 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1403,8 +1403,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (vertexCount == 0 || indexCount == 0) { - m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1}", - Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh"); + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}", + Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString()); m_hasOBB = false; m_OBBOffset = Vector3.Zero; -- cgit v1.1 From 62244b5ea5d9fcbe24e35906517fb02022cdcc7c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 24 Nov 2012 22:46:24 +0000 Subject: don't zero constant force and torque in selection --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index dc247a9..7dddab6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -84,7 +84,7 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 _position; private Vector3 _velocity; - private Vector3 _torque; + private Vector3 m_torque; private Vector3 m_lastVelocity; private Vector3 m_lastposition; private Vector3 m_rotationalVelocity; @@ -597,7 +597,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!IsPhysical || Body == IntPtr.Zero) return Vector3.Zero; - return _torque; + return m_torque; } set @@ -2425,10 +2425,10 @@ namespace OpenSim.Region.Physics.OdePlugin { if (!childPrim) { - m_force = Vector3.Zero; +// m_force = Vector3.Zero; m_forceacc = Vector3.Zero; m_angularForceacc = Vector3.Zero; - _torque = Vector3.Zero; +// m_torque = Vector3.Zero; _velocity = Vector3.Zero; _acceleration = Vector3.Zero; m_rotationalVelocity = Vector3.Zero; @@ -2968,7 +2968,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodyEnable(Body); } - _torque = newtorque; + m_torque = newtorque; } } @@ -3364,7 +3364,7 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 trq; - trq = _torque; + trq = m_torque; trq += m_angularForceacc; m_angularForceacc = Vector3.Zero; if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) -- cgit v1.1 From c50fda8bf56b1935805164e6803ab82532ea5418 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 1 Dec 2012 22:58:52 +0000 Subject: adjust avatar standing Z position --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index f5bf05d..94ed663 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -85,8 +85,12 @@ namespace OpenSim.Region.Physics.OdePlugin public float PID_D = 800.0f; public float PID_P = 900.0f; //private static float POSTURE_SERVO = 10000.0f; + public float CAPSULE_RADIUS = 0.37f; public float CAPSULE_LENGTH = 2.140599f; + + const float CAP_OFFSET = -.2f; // compensation of SL size offset plus spheric collision shape bottom + public float walkDivisor = 1.3f; public float runDivisor = 0.8f; private bool flying = false; @@ -139,6 +143,8 @@ namespace OpenSim.Region.Physics.OdePlugin float mu; + + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) { m_uuid = UUID.Random(); @@ -177,7 +183,7 @@ namespace OpenSim.Region.Physics.OdePlugin walkDivisor = walk_divisor; runDivisor = rundivisor; - CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f; + CAPSULE_LENGTH = size.Z - CAPSULE_RADIUS + CAP_OFFSET; //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); m_isPhysical = false; // current status: no ODE information exists @@ -422,7 +428,8 @@ namespace OpenSim.Region.Physics.OdePlugin { get { float d = CAPSULE_RADIUS * 2; - return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); } + return new Vector3(d, d, (CAPSULE_LENGTH + CAPSULE_RADIUS - CAP_OFFSET)); + } set { if (value.IsFinite()) @@ -837,8 +844,7 @@ namespace OpenSim.Region.Physics.OdePlugin // colide with land d.AABB aabb; d.GeomGetAABB(Shell, out aabb); - float chrminZ = aabb.MinZ; - + float chrminZ = aabb.MinZ - 0.04f; // move up a bit Vector3 posch = localpos; float ftmp; @@ -1224,7 +1230,7 @@ namespace OpenSim.Region.Physics.OdePlugin { float caplen = Size.Z; - caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f; + caplen = caplen - CAPSULE_RADIUS + CAP_OFFSET; if (caplen != CAPSULE_LENGTH) { -- cgit v1.1 From aa00308b10228cd2c5a04a41fc5b3da8009cd0fd Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 2 Dec 2012 03:58:40 +0000 Subject: *TEST* reduce all mesh vertices resolution to 1e-5. This reduces number of unique vertices only originated by math errors in PrimMesher --- OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index 1e9b8bc..0476b28 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -205,12 +205,28 @@ namespace OpenSim.Region.Physics.Meshing } + private float fRound(float f) + { + int i = (int)(1e5f * f +0.5f); + return ((float)i * 1e-5f); + } public void Add(Triangle triangle) { if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) throw new NotSupportedException("Attempt to Add to a pinned Mesh"); + // round down + triangle.v1.X = fRound(triangle.v1.X); + triangle.v1.Y = fRound(triangle.v1.Y); + triangle.v1.Z = fRound(triangle.v1.Z); + triangle.v2.X = fRound(triangle.v2.X); + triangle.v2.Y = fRound(triangle.v2.Y); + triangle.v2.Z = fRound(triangle.v2.Z); + triangle.v3.X = fRound(triangle.v3.X); + triangle.v3.Y = fRound(triangle.v3.Y); + triangle.v3.Z = fRound(triangle.v3.Z); + if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z) -- cgit v1.1 From f21d9908200b4bb21f8002e9c87968767ab77bdb Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 2 Dec 2012 05:02:33 +0000 Subject: fix vertex rounding direction --- OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index 0476b28..b67422f 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -207,7 +207,14 @@ namespace OpenSim.Region.Physics.Meshing private float fRound(float f) { - int i = (int)(1e5f * f +0.5f); + int i; + if (f == 0f) + return f; + else if (f > 0f) + i = (int)(1e5f * f + 0.5f); + else + i = (int)(1e5f * f - 0.5f); + return ((float)i * 1e-5f); } -- cgit v1.1 From 8aa5fdb6a3f1e4b349757df5d9fcc06ab8dfdb64 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 3 Dec 2012 17:05:05 +0000 Subject: *TEST* diferent avatar collider --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 369 ++++++++++++++++----- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 69 ++-- 2 files changed, 337 insertions(+), 101 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 94ed663..3d5be3e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -79,6 +79,8 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 _target_velocity; private Vector3 _acceleration; private Vector3 m_rotationalVelocity; + private Vector3 m_size; + private Quaternion m_orientation; private float m_mass = 80f; public float m_density = 60f; private bool m_pidControllerActive = true; @@ -86,10 +88,17 @@ namespace OpenSim.Region.Physics.OdePlugin public float PID_P = 900.0f; //private static float POSTURE_SERVO = 10000.0f; - public float CAPSULE_RADIUS = 0.37f; - public float CAPSULE_LENGTH = 2.140599f; - const float CAP_OFFSET = -.2f; // compensation of SL size offset plus spheric collision shape bottom + private float m_invElipSizeX; + private float m_invElipSizeY; + + private float feetOff = 0; + private float feetSZ = 0.5f; + const float feetScale = 0.9f; + const float invFeetScale = 1.0f / 0.9f; + const float sizeZAdjust = 0.15f; + private float boneOff = 0; + public float walkDivisor = 1.3f; public float runDivisor = 0.8f; @@ -127,10 +136,16 @@ namespace OpenSim.Region.Physics.OdePlugin // we do land collisions not ode | CollisionCategories.Land); public IntPtr Body = IntPtr.Zero; private OdeScene _parent_scene; - public IntPtr Shell = IntPtr.Zero; + public IntPtr topbox = IntPtr.Zero; + public IntPtr midbox = IntPtr.Zero; + public IntPtr feetbox = IntPtr.Zero; + public IntPtr bonebox = IntPtr.Zero; + public IntPtr Amotor = IntPtr.Zero; + public d.Mass ShellMass; -// public bool collidelock = false; + + public int m_eventsubscription = 0; private int m_cureventsubscription = 0; @@ -145,7 +160,7 @@ namespace OpenSim.Region.Physics.OdePlugin - public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pid_d, float pid_p, float density, float walk_divisor, float rundivisor) { m_uuid = UUID.Random(); @@ -171,9 +186,20 @@ namespace OpenSim.Region.Physics.OdePlugin PID_D = pid_d; PID_P = pid_p; - CAPSULE_RADIUS = capsule_radius; + + m_size.X = pSize.X; + m_size.Y = pSize.Y; + m_size.Z = pSize.Z; + + if(m_size.X <0.01f) + m_size.X = 0.01f; + if(m_size.Y <0.01f) + m_size.Y = 0.01f; + if(m_size.Z <0.01f) + m_size.Z = 0.01f; + + m_orientation = Quaternion.Identity; m_density = density; - m_mass = 80f; // sure we have a default // force lower density for testing m_density = 3.0f; @@ -183,8 +209,7 @@ namespace OpenSim.Region.Physics.OdePlugin walkDivisor = walk_divisor; runDivisor = rundivisor; - CAPSULE_LENGTH = size.Z - CAPSULE_RADIUS + CAP_OFFSET; - //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default m_isPhysical = false; // current status: no ODE information exists @@ -426,14 +451,21 @@ namespace OpenSim.Region.Physics.OdePlugin /// public override Vector3 Size { - get { - float d = CAPSULE_RADIUS * 2; - return new Vector3(d, d, (CAPSULE_LENGTH + CAPSULE_RADIUS - CAP_OFFSET)); + get + { + return m_size; } set { if (value.IsFinite()) { + if(value.X <0.01f) + value.X = 0.01f; + if(value.Y <0.01f) + value.Y = 0.01f; + if(value.Z <0.01f) + value.Z = 0.01f; + AddChange(changes.Size, value); } else @@ -459,8 +491,7 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH)); - return m_density * AVvolume; + return m_density * m_size.X * m_size.Y * m_size.Z; } } public override void link(PhysicsActor obj) @@ -578,9 +609,14 @@ namespace OpenSim.Region.Physics.OdePlugin public override Quaternion Orientation { - get { return Quaternion.Identity; } + get { return m_orientation; } set { + // fakeori = value; + // givefakeori++; + + value.Normalize(); + AddChange(changes.Orientation, value); } } @@ -632,32 +668,65 @@ namespace OpenSim.Region.Physics.OdePlugin AddChange(changes.Momentum, momentum); } - - // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access - // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only - // place that is safe to call this routine AvatarGeomAndBodyCreation. private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) { + // sizes one day should came from visual parameters + float sz = m_size.Z + sizeZAdjust; + + m_invElipSizeX = 1.0f / m_size.X; + m_invElipSizeY = 1.0f / m_size.Y; + + float topsx = m_size.X; + float midsx = m_size.X; + float feetsx = m_size.X * feetScale; + float bonesx = feetsx * 0.2f; + + float topsy = m_size.Y * 0.5f; + float midsy = m_size.Y; + float feetsy = m_size.Y * feetScale; + float bonesy = feetsy * 0.2f; + + float topsz = sz * 0.15f; + float feetsz = sz * 0.3f; + if (feetsz > 0.6f) + feetsz = 0.6f; + + float midsz = sz - topsz - feetsz; + float bonesz = sz; + + float bot = -sz * 0.5f; + + boneOff = bot + 0.3f; + + float feetz = bot + feetsz * 0.5f; + bot += feetsz; + + feetOff = bot; + feetSZ = feetsz; + + float midz = bot + midsz * 0.5f; + bot += midsz; + float topz = bot + topsz * 0.5f; + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); - if (CAPSULE_LENGTH <= 0) - { - m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); - CAPSULE_LENGTH = 0.01f; - } + feetbox = d.CreateBox(_parent_scene.ActiveSpace, feetsx, feetsy, feetsz); + d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); - if (CAPSULE_RADIUS <= 0) - { - m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); - CAPSULE_RADIUS = 0.01f; + midbox = d.CreateBox(_parent_scene.ActiveSpace, midsx, midsy, midsz); + d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); - } - Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); + topbox = d.CreateBox(_parent_scene.ActiveSpace, topsx, topsy, topsz); + d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); - d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories); - d.GeomSetCollideBits(Shell, (uint)m_collisionFlags); + bonebox = d.CreateBox(_parent_scene.ActiveSpace, bonesx, bonesy, bonesz); + d.GeomSetCategoryBits(bonebox, (uint)m_collisionCategories); + d.GeomSetCollideBits(bonebox, (uint)m_collisionFlags); - d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); + d.MassSetBox(out ShellMass, m_density, m_size.X , m_size.Y, m_size.Z); m_mass = ShellMass.mass; // update mass @@ -688,7 +757,14 @@ namespace OpenSim.Region.Physics.OdePlugin _position.Z = npositionZ; d.BodySetMass(Body, ref ShellMass); - d.GeomSetBody(Shell, Body); + d.GeomSetBody(feetbox, Body); + d.GeomSetBody(midbox, Body); + d.GeomSetBody(topbox, Body); + d.GeomSetBody(bonebox, Body); + + d.GeomSetOffsetPosition(feetbox, 0, 0, feetz); + d.GeomSetOffsetPosition(midbox, 0, 0, midz); + d.GeomSetOffsetPosition(topbox, 0, 0, topz); // The purpose of the AMotor here is to keep the avatar's physical // surrogate from rotating while moving @@ -748,15 +824,152 @@ namespace OpenSim.Region.Physics.OdePlugin Body = IntPtr.Zero; } - //kill the Geometry - if (Shell != IntPtr.Zero) + //kill the Geoms + if (topbox != IntPtr.Zero) { -// _parent_scene.geom_name_map.Remove(Shell); - _parent_scene.actor_name_map.Remove(Shell); + _parent_scene.actor_name_map.Remove(topbox); _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); - d.GeomDestroy(Shell); - Shell = IntPtr.Zero; + d.GeomDestroy(topbox); + topbox = IntPtr.Zero; + } + if (midbox != IntPtr.Zero) + { + _parent_scene.actor_name_map.Remove(midbox); + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + d.GeomDestroy(midbox); + midbox = IntPtr.Zero; + } + if (feetbox != IntPtr.Zero) + { + _parent_scene.actor_name_map.Remove(feetbox); + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + d.GeomDestroy(feetbox); + feetbox = IntPtr.Zero; + } + + if (bonebox != IntPtr.Zero) + { + _parent_scene.actor_name_map.Remove(bonebox); + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + d.GeomDestroy(bonebox); + bonebox = IntPtr.Zero; + } + + } + + public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact) + { + + if (me == bonebox) // inner bone + { + if (contact.pos.Z - _position.Z < boneOff) + IsColliding = true; + return true; + } + + if (me == topbox) // keep a box head + return true; + + // rotate elipsoide assuming only rotation around Z + float ca = m_orientation.W * m_orientation.W - m_orientation.Z * m_orientation.Z; + float sa = 2 * m_orientation.W * m_orientation.Z; + + float isx; + float isy; + + if (me == feetbox) // feet have narrow bounds + { + + isx = m_invElipSizeX * invFeetScale; + isy = m_invElipSizeY * invFeetScale; + } + else + { + isx = m_invElipSizeX; + isy = m_invElipSizeY; + } + + float a = isx * ca - isy * sa; + float b = isx * sa + isy * ca; + + float offx = contact.pos.X - _position.X; + float er = offx * a; + er *= er; + + float offy = contact.pos.Y - _position.Y; + float ty = offy * b; + er += ty * ty; + + if (me == midbox) + { + if (er > 4.0f) // no collision + return false; + if (er < 0.2f) + return true; + + float t = offx * offx + offy * offy; + t = (float)Math.Sqrt(t); + t = 1 / t; + offx *= t; + offy *= t; + + if (reverse) + { + contact.normal.X = offx; + contact.normal.Y = offy; + } + else + { + contact.normal.X = -offx; + contact.normal.Y = -offy; + } + + contact.normal.Z = 0; + return true; + } + + else if (me == feetbox) + { + float c = feetSZ * 2; + float h = contact.pos.Z - _position.Z; + float offz = h - feetOff; // distance from top of feetbox + + float tz = offz / c; + er += tz * tz; + + if (er > 4.0f) // no collision + return false; + + if (er > 0.2f) + { + float t = offx * offx + offy * offy + offz * offz; + t = (float)Math.Sqrt(t); + t = 1 / t; + offx *= t; + offy *= t; + offz *= t; + + if (reverse) + { + contact.normal.X = offx; + contact.normal.Y = offy; + contact.normal.Z = offz; + } + else + { + contact.normal.X = -offx; + contact.normal.Y = -offy; + contact.normal.Z = -offz; + } + } + + if(h < boneOff) + IsColliding = true; } + else + return false; + + return true; } /// @@ -776,10 +989,10 @@ namespace OpenSim.Region.Physics.OdePlugin // so force it back to identity d.Quaternion qtmp; - qtmp.W = 1; - qtmp.X = 0; - qtmp.Y = 0; - qtmp.Z = 0; + qtmp.W = m_orientation.W; + qtmp.X = m_orientation.X; + qtmp.Y = m_orientation.Y; + qtmp.Z = m_orientation.Z; d.BodySetQuaternion(Body, ref qtmp); if (m_pidControllerActive == false) @@ -843,7 +1056,7 @@ namespace OpenSim.Region.Physics.OdePlugin //****************************************** // colide with land d.AABB aabb; - d.GeomGetAABB(Shell, out aabb); + d.GeomGetAABB(feetbox, out aabb); float chrminZ = aabb.MinZ - 0.04f; // move up a bit Vector3 posch = localpos; @@ -1182,20 +1395,14 @@ namespace OpenSim.Region.Physics.OdePlugin { if (NewStatus) { - // Create avatar capsule and related ODE data - if ((Shell != IntPtr.Zero)) - { - // a lost shell ? - m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " - + (Shell != IntPtr.Zero ? "Shell " : "") - + (Body != IntPtr.Zero ? "Body " : "") - + (Amotor != IntPtr.Zero ? "Amotor " : "")); - AvatarGeomAndBodyDestroy(); - } + AvatarGeomAndBodyDestroy(); AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); - _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this; _parent_scene.AddCharacter(this); } else @@ -1224,37 +1431,29 @@ namespace OpenSim.Region.Physics.OdePlugin { } - private void changeSize(Vector3 Size) + private void changeSize(Vector3 pSize) { - if (Size.IsFinite()) + if (pSize.IsFinite()) { - float caplen = Size.Z; + // for now only look to Z changes since viewers also don't change X and Y + if (pSize.Z != m_size.Z) + { + AvatarGeomAndBodyDestroy(); - caplen = caplen - CAPSULE_RADIUS + CAP_OFFSET; - if (caplen != CAPSULE_LENGTH) - { - if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) - { - AvatarGeomAndBodyDestroy(); + float oldsz = m_size.Z; + m_size = pSize; - float prevCapsule = CAPSULE_LENGTH; - CAPSULE_LENGTH = caplen; - AvatarGeomAndBodyCreation(_position.X, _position.Y, - _position.Z + (CAPSULE_LENGTH - prevCapsule) * 0.5f); + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (m_size.Z - oldsz) * 0.5f); - Velocity = Vector3.Zero; + Velocity = Vector3.Zero; - _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; - } - else - { - m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " - + (Shell == IntPtr.Zero ? "Shell " : "") - + (Body == IntPtr.Zero ? "Body " : "") - + (Amotor == IntPtr.Zero ? "Amotor " : "")); - } + _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this; } m_freemove = false; m_pidControllerActive = true; @@ -1276,6 +1475,14 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeOrientation(Quaternion newOri) { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + float t = d.JointGetAMotorAngle(Amotor, 2); + d.BodySetQuaternion(Body,ref myrot); + m_orientation = newOri; } private void changeVelocity(Vector3 newVel) @@ -1365,7 +1572,7 @@ namespace OpenSim.Region.Physics.OdePlugin public bool DoAChange(changes what, object arg) { - if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove) + if (topbox == IntPtr.Zero && what != changes.Add && what != changes.Remove) { return false; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 54bc29f..003a91c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -840,6 +840,8 @@ namespace OpenSim.Region.Physics.OdePlugin { case (int)ActorTypes.Agent: { + dop1foot = true; + AvanormOverride = true; Vector3 tmp = p2.Position - p1.Position; normoverride = p2.Velocity - p1.Velocity; @@ -883,6 +885,10 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: + + + dop2foot = true; + AvanormOverride = true; Vector3 tmp = p2.Position - p1.Position; @@ -1017,6 +1023,7 @@ namespace OpenSim.Region.Physics.OdePlugin IntPtr Joint; int i = 0; + int ncontacts = 0; while(true) { @@ -1031,7 +1038,28 @@ namespace OpenSim.Region.Physics.OdePlugin else { + if(dop1foot) + { + if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact))) + { + if (++i >= count) + break; + else + continue; + } + } + else if(dop2foot) + { + if(!(((OdeCharacter) p2).Collide(g2,true,ref curContact))) + { + if (++i >= count) + break; + else + continue; + } + } +/* if (AvanormOverride) { if (curContact.depth > 0.3f) @@ -1081,34 +1109,31 @@ namespace OpenSim.Region.Physics.OdePlugin { float sz = p2.Size.Z; Vector3 vtmp = p2.Position; - float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; + vtmp.Z -= sz * 0.5f; + vtmp.Z += 0.5f; + float ppos = vtmp.Z - curContact.pos.Z; if (ppos > 0f) { if (!p2.Flying) { - d.AABB aabb; - d.GeomGetAABB(g1, out aabb); float tmp = vtmp.Z - sz * .18f; - - if (aabb.MaxZ < tmp) - { - vtmp.X = curContact.pos.X - vtmp.X; - vtmp.Y = curContact.pos.Y - vtmp.Y; - vtmp.Z = -0.2f; - vtmp.Normalize(); - curContact.normal.X = vtmp.X; - curContact.normal.Y = vtmp.Y; - curContact.normal.Z = vtmp.Z; - } + vtmp.X = curContact.pos.X - vtmp.X; + vtmp.Y = curContact.pos.Y - vtmp.Y; + vtmp.Z = curContact.pos.Z - vtmp.Z; + vtmp.Normalize(); + curContact.normal.X = vtmp.X; + curContact.normal.Y = vtmp.Y; + curContact.normal.Z = vtmp.Z; } } - else +// else p2.IsColliding = true; } } } - +*/ + ncontacts++; Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); d.JointAttach(Joint, b1, b2); @@ -1134,7 +1159,8 @@ namespace OpenSim.Region.Physics.OdePlugin } } - collision_accounting_events(p1, p2, maxDepthContact); + if(ncontacts > 0) + collision_accounting_events(p1, p2, maxDepthContact); /* if (notskipedcount > geomContactPointsStartthrottle) @@ -1234,14 +1260,17 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdeCharacter chr in _characters) { - if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) + if (chr == null || chr.Body == IntPtr.Zero) continue; chr.IsColliding = false; // chr.CollidingGround = false; not done here chr.CollidingObj = false; // do colisions with static space - d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback); + d.SpaceCollide2(StaticSpace, chr.topbox, IntPtr.Zero, nearCallback); + d.SpaceCollide2(StaticSpace, chr.midbox, IntPtr.Zero, nearCallback); + d.SpaceCollide2(StaticSpace, chr.feetbox, IntPtr.Zero, nearCallback); + d.SpaceCollide2(StaticSpace, chr.bonebox, IntPtr.Zero, nearCallback); // no coll with gnd } } @@ -1334,7 +1363,7 @@ namespace OpenSim.Region.Physics.OdePlugin pos.X = position.X; pos.Y = position.Y; pos.Z = position.Z; - OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avDensity, avMovementDivisorWalk, avMovementDivisorRun); newAv.Flying = isFlying; newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; -- cgit v1.1 From fc1be7e41fc7dd23c5396665f7b464de45590368 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 4 Dec 2012 01:54:37 +0000 Subject: raise standing avatar a bit to reduce knees bending on some collisions. reduce head size a bit --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 3d5be3e..f33fdb4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -96,7 +96,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float feetSZ = 0.5f; const float feetScale = 0.9f; const float invFeetScale = 1.0f / 0.9f; - const float sizeZAdjust = 0.15f; + const float sizeZAdjust = 0.18f; private float boneOff = 0; @@ -676,12 +676,12 @@ namespace OpenSim.Region.Physics.OdePlugin m_invElipSizeX = 1.0f / m_size.X; m_invElipSizeY = 1.0f / m_size.Y; - float topsx = m_size.X; + float topsx = m_size.X * 0.9f; float midsx = m_size.X; float feetsx = m_size.X * feetScale; float bonesx = feetsx * 0.2f; - float topsy = m_size.Y * 0.5f; + float topsy = m_size.Y * 0.4f; float midsy = m_size.Y; float feetsy = m_size.Y * feetScale; float bonesy = feetsy * 0.2f; @@ -1057,7 +1057,7 @@ namespace OpenSim.Region.Physics.OdePlugin // colide with land d.AABB aabb; d.GeomGetAABB(feetbox, out aabb); - float chrminZ = aabb.MinZ - 0.04f; // move up a bit + float chrminZ = aabb.MinZ - 0.02f; // move up a bit Vector3 posch = localpos; float ftmp; -- cgit v1.1 From b6d29aa124cf1ec1c1dbe6874720e6cc39faf06f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 4 Dec 2012 02:46:40 +0000 Subject: move characters (avatars) to own collision space, also fixing a problem with previus code that was still assuming the avatar is g2 --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 18 +++++----- .../UbitOdePlugin/ODERayCastRequestManager.cs | 3 ++ OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 40 +++++++++++++++++++--- 3 files changed, 47 insertions(+), 14 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index f33fdb4..925900f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -708,21 +708,21 @@ namespace OpenSim.Region.Physics.OdePlugin bot += midsz; float topz = bot + topsz * 0.5f; - _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); - feetbox = d.CreateBox(_parent_scene.ActiveSpace, feetsx, feetsy, feetsz); + feetbox = d.CreateBox(_parent_scene.CharsSpace, feetsx, feetsy, feetsz); d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); - midbox = d.CreateBox(_parent_scene.ActiveSpace, midsx, midsy, midsz); + midbox = d.CreateBox(_parent_scene.CharsSpace, midsx, midsy, midsz); d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); - topbox = d.CreateBox(_parent_scene.ActiveSpace, topsx, topsy, topsz); + topbox = d.CreateBox(_parent_scene.CharsSpace, topsx, topsy, topsz); d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); - bonebox = d.CreateBox(_parent_scene.ActiveSpace, bonesx, bonesy, bonesz); + bonebox = d.CreateBox(_parent_scene.CharsSpace, bonesx, bonesy, bonesz); d.GeomSetCategoryBits(bonebox, (uint)m_collisionCategories); d.GeomSetCollideBits(bonebox, (uint)m_collisionFlags); @@ -828,21 +828,21 @@ namespace OpenSim.Region.Physics.OdePlugin if (topbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(topbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); d.GeomDestroy(topbox); topbox = IntPtr.Zero; } if (midbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(midbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); d.GeomDestroy(midbox); midbox = IntPtr.Zero; } if (feetbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(feetbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); d.GeomDestroy(feetbox); feetbox = IntPtr.Zero; } @@ -850,7 +850,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (bonebox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(bonebox); - _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); d.GeomDestroy(bonebox); bonebox = IntPtr.Zero; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 799a324..f449099 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -324,7 +324,10 @@ namespace OpenSim.Region.Physics.OdePlugin { // Collide tests if ((CurrentRayFilter & FilterActiveSpace) != 0) + { d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 003a91c..07987d1 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -299,6 +299,7 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr TopSpace; // the global space public IntPtr ActiveSpace; // space for active prims + public IntPtr CharsSpace; // space for active prims public IntPtr StaticSpace; // space for the static things around public IntPtr GroundSpace; // space for ground @@ -372,21 +373,25 @@ namespace OpenSim.Region.Physics.OdePlugin // now the major subspaces ActiveSpace = d.HashSpaceCreate(TopSpace); + CharsSpace = d.HashSpaceCreate(TopSpace); StaticSpace = d.HashSpaceCreate(TopSpace); GroundSpace = d.HashSpaceCreate(TopSpace); } catch { // i must RtC#FM + // i did! } d.HashSpaceSetLevels(TopSpace, -2, 8); d.HashSpaceSetLevels(ActiveSpace, -2, 8); + d.HashSpaceSetLevels(CharsSpace, -4, 3); d.HashSpaceSetLevels(StaticSpace, -2, 8); d.HashSpaceSetLevels(GroundSpace, 0, 8); // demote to second level d.SpaceSetSublevel(ActiveSpace, 1); + d.SpaceSetSublevel(CharsSpace, 1); d.SpaceSetSublevel(StaticSpace, 1); d.SpaceSetSublevel(GroundSpace, 1); @@ -396,11 +401,24 @@ namespace OpenSim.Region.Physics.OdePlugin CollisionCategories.Phantom | CollisionCategories.VolumeDtc )); - d.GeomSetCollideBits(ActiveSpace, 0); + d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(CharsSpace, 0); + d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | CollisionCategories.Geom | - CollisionCategories.Land | - CollisionCategories.Water | +// CollisionCategories.Land | +// CollisionCategories.Water | CollisionCategories.Phantom | CollisionCategories.VolumeDtc )); @@ -1271,6 +1289,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceCollide2(StaticSpace, chr.midbox, IntPtr.Zero, nearCallback); d.SpaceCollide2(StaticSpace, chr.feetbox, IntPtr.Zero, nearCallback); d.SpaceCollide2(StaticSpace, chr.bonebox, IntPtr.Zero, nearCallback); + + // chars with chars + d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); // no coll with gnd } } @@ -1312,16 +1333,25 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); } } - // finally colide active things amoung them + // colide active amoung them try { d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); } catch (AccessViolationException) { + m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space"); + } + // and with chars + try + { + d.SpaceCollide2(ActiveSpace, CharsSpace,IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { m_log.Warn("[PHYSICS]: Unable to collide in Active space"); } -// _perloopContact.Clear(); + // _perloopContact.Clear(); } #endregion -- cgit v1.1 From de3180a63ecb89971321b7dce60dd86703f87e6d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 5 Dec 2012 23:19:18 +0000 Subject: avatar collision plane send to viewer is only relative to feet. change avatar collider, just rounding the boxes, etc --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 2 + .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 155 +++++++++------------ OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 18 +-- 3 files changed, 74 insertions(+), 101 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 5af6373..e1168bd 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -61,6 +61,7 @@ namespace OpenSim.Region.Physics.Manager public Vector3 SurfaceNormal; public float PenetrationDepth; public float RelativeSpeed; + public bool CharacterFeet; public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth) { @@ -68,6 +69,7 @@ namespace OpenSim.Region.Physics.Manager SurfaceNormal = surfaceNormal; PenetrationDepth = penetrationDepth; RelativeSpeed = 0f; // for now let this one be set explicity + CharacterFeet = true; // keep other plugins work as before } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 925900f..fd6b8aa 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -84,18 +84,15 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_mass = 80f; public float m_density = 60f; private bool m_pidControllerActive = true; - public float PID_D = 800.0f; - public float PID_P = 900.0f; - //private static float POSTURE_SERVO = 10000.0f; - - private float m_invElipSizeX; - private float m_invElipSizeY; + const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80)) + const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80)) + public float PID_D; + public float PID_P; private float feetOff = 0; private float feetSZ = 0.5f; - const float feetScale = 0.9f; - const float invFeetScale = 1.0f / 0.9f; + const float feetScale = 0.8f; const float sizeZAdjust = 0.18f; private float boneOff = 0; @@ -160,7 +157,7 @@ namespace OpenSim.Region.Physics.OdePlugin - public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pid_d, float pid_p, float density, float walk_divisor, float rundivisor) + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float density, float walk_divisor, float rundivisor) { m_uuid = UUID.Random(); @@ -184,8 +181,6 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene = parent_scene; - PID_D = pid_d; - PID_P = pid_p; m_size.X = pSize.X; m_size.Y = pSize.Y; @@ -204,6 +199,8 @@ namespace OpenSim.Region.Physics.OdePlugin // force lower density for testing m_density = 3.0f; + m_density *= 1.4f; // scale to have mass similar to capsule + mu = parent_scene.AvatarFriction; walkDivisor = walk_divisor; @@ -211,6 +208,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default + PID_D = basePID_D * m_mass / parent_scene.ODE_STEPSIZE; + PID_P = basePID_P * m_mass / parent_scene.ODE_STEPSIZE; + m_isPhysical = false; // current status: no ODE information exists Name = avName; @@ -491,7 +491,7 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - return m_density * m_size.X * m_size.Y * m_size.Z; + return m_mass; } } public override void link(PhysicsActor obj) @@ -671,23 +671,22 @@ namespace OpenSim.Region.Physics.OdePlugin private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) { // sizes one day should came from visual parameters + float sx = m_size.X; + float sy = m_size.Y; float sz = m_size.Z + sizeZAdjust; - m_invElipSizeX = 1.0f / m_size.X; - m_invElipSizeY = 1.0f / m_size.Y; + float topsx = sx * 0.9f; + float midsx = sx; + float feetsx = sx * feetScale; + float bonesx = sx * 0.2f; - float topsx = m_size.X * 0.9f; - float midsx = m_size.X; - float feetsx = m_size.X * feetScale; - float bonesx = feetsx * 0.2f; - - float topsy = m_size.Y * 0.4f; - float midsy = m_size.Y; - float feetsy = m_size.Y * feetScale; + float topsy = sy * 0.4f; + float midsy = sy; + float feetsy = sy * feetScale * 0.8f; float bonesy = feetsy * 0.2f; float topsz = sz * 0.15f; - float feetsz = sz * 0.3f; + float feetsz = sz * 0.45f; if (feetsz > 0.6f) feetsz = 0.6f; @@ -726,22 +725,12 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(bonebox, (uint)m_collisionCategories); d.GeomSetCollideBits(bonebox, (uint)m_collisionFlags); - d.MassSetBox(out ShellMass, m_density, m_size.X , m_size.Y, m_size.Z); - - m_mass = ShellMass.mass; // update mass - - // rescale PID parameters - PID_D = _parent_scene.avPIDD; - PID_P = _parent_scene.avPIDP; + m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass - // rescale PID parameters so that this aren't affected by mass - // and so don't get unstable for some masses - // also scale by ode time step so you don't need to refix them + d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z); - PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps - PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; - PID_P /= 50 * 80; - PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; + PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE; + PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE; Body = d.BodyCreate(_parent_scene.world); @@ -857,8 +846,9 @@ namespace OpenSim.Region.Physics.OdePlugin } - public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact) + public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) { + feetcollision = false; if (me == bonebox) // inner bone { @@ -870,44 +860,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (me == topbox) // keep a box head return true; - // rotate elipsoide assuming only rotation around Z - float ca = m_orientation.W * m_orientation.W - m_orientation.Z * m_orientation.Z; - float sa = 2 * m_orientation.W * m_orientation.Z; - - float isx; - float isy; - - if (me == feetbox) // feet have narrow bounds - { - - isx = m_invElipSizeX * invFeetScale; - isy = m_invElipSizeY * invFeetScale; - } - else - { - isx = m_invElipSizeX; - isy = m_invElipSizeY; - } - - float a = isx * ca - isy * sa; - float b = isx * sa + isy * ca; - + float t; float offx = contact.pos.X - _position.X; - float er = offx * a; - er *= er; - float offy = contact.pos.Y - _position.Y; - float ty = offy * b; - er += ty * ty; if (me == midbox) { - if (er > 4.0f) // no collision - return false; - if (er < 0.2f) - return true; - - float t = offx * offx + offy * offy; + t = offx * offx + offy * offy; t = (float)Math.Sqrt(t); t = 1 / t; offx *= t; @@ -930,40 +889,51 @@ namespace OpenSim.Region.Physics.OdePlugin else if (me == feetbox) { - float c = feetSZ * 2; float h = contact.pos.Z - _position.Z; - float offz = h - feetOff; // distance from top of feetbox - float tz = offz / c; - er += tz * tz; + if (Math.Abs(contact.normal.Z) > 0.95f) + { + feetcollision = true; + if (h < boneOff) + IsColliding = true; + return true; + } + + float offz = h - feetOff; // distance from top of feetbox - if (er > 4.0f) // no collision + if (offz > 0) return false; - if (er > 0.2f) + if (offz > -0.01) + { + offx = 0; + offy = 0; + offz = -1.0f; + } + else { - float t = offx * offx + offy * offy + offz * offz; + t = offx * offx + offy * offy + offz * offz; t = (float)Math.Sqrt(t); t = 1 / t; offx *= t; offy *= t; offz *= t; - - if (reverse) - { - contact.normal.X = offx; - contact.normal.Y = offy; - contact.normal.Z = offz; - } - else - { - contact.normal.X = -offx; - contact.normal.Y = -offy; - contact.normal.Z = -offz; - } } - if(h < boneOff) + if (reverse) + { + contact.normal.X = offx; + contact.normal.Y = offy; + contact.normal.Z = offz; + } + else + { + contact.normal.X = -offx; + contact.normal.Y = -offy; + contact.normal.Z = -offz; + } + feetcollision = true; + if (h < boneOff) IsColliding = true; } else @@ -1105,6 +1075,7 @@ namespace OpenSim.Region.Physics.OdePlugin contact.SurfaceNormal.Y = 0f; contact.SurfaceNormal.Z = -1f; contact.RelativeSpeed = -vel.Z; + contact.CharacterFeet = true; AddCollisionEvent(0, contact); vec.Z *= 0.5f; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 07987d1..2b4d368 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -224,9 +224,6 @@ namespace OpenSim.Region.Physics.OdePlugin // private IntPtr WaterHeightmapData = IntPtr.Zero; // private GCHandle WaterMapHandler = new GCHandle(); - public float avPIDD = 2200f; // make it visible - public float avPIDP = 900f; // make it visible - private float avCapRadius = 0.37f; private float avDensity = 3f; private float avMovementDivisorWalk = 1.3f; private float avMovementDivisorRun = 0.8f; @@ -486,7 +483,6 @@ namespace OpenSim.Region.Physics.OdePlugin avDensity = physicsconfig.GetFloat("av_density", avDensity); avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); - avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius); contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); @@ -1040,6 +1036,8 @@ namespace OpenSim.Region.Physics.OdePlugin IntPtr Joint; + bool FeetCollision = false; + int i = 0; int ncontacts = 0; while(true) @@ -1058,7 +1056,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if(dop1foot) { - if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact))) + if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision))) { if (++i >= count) break; @@ -1068,7 +1066,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else if(dop2foot) { - if(!(((OdeCharacter) p2).Collide(g2,true,ref curContact))) + if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision))) { if (++i >= count) break; @@ -1177,9 +1175,11 @@ namespace OpenSim.Region.Physics.OdePlugin } } - if(ncontacts > 0) + if (ncontacts > 0) + { + maxDepthContact.CharacterFeet = FeetCollision; collision_accounting_events(p1, p2, maxDepthContact); - + } /* if (notskipedcount > geomContactPointsStartthrottle) { @@ -1393,7 +1393,7 @@ namespace OpenSim.Region.Physics.OdePlugin pos.X = position.X; pos.Y = position.Y; pos.Z = position.Z; - OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avDensity, avMovementDivisorWalk, avMovementDivisorRun); newAv.Flying = isFlying; newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; -- cgit v1.1 From d2499c4c314b290c42f454913305d97c6eec92d6 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 7 Dec 2012 15:54:46 +0000 Subject: *TEST* Use new avatar size in ubitODE. --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 5 +++ .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 48 ++++++++++++++++++++-- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 1 + 3 files changed, 50 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index e1168bd..0405dad 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -175,6 +175,11 @@ namespace OpenSim.Region.Physics.Manager public abstract Vector3 Size { get; set; } + public virtual void setAvatarSize(Vector3 size, float feetOffset) + { + Size = size; + } + public virtual bool Phantom { get; set; } public virtual bool IsVolumeDtc diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index fd6b8aa..9c245e6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -90,13 +90,12 @@ namespace OpenSim.Region.Physics.OdePlugin public float PID_D; public float PID_P; + private float m_feetOffset = 0; private float feetOff = 0; private float feetSZ = 0.5f; const float feetScale = 0.8f; - const float sizeZAdjust = 0.18f; private float boneOff = 0; - public float walkDivisor = 1.3f; public float runDivisor = 0.8f; private bool flying = false; @@ -475,6 +474,28 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public override void setAvatarSize(Vector3 size, float feetOffset) + { + if (size.IsFinite()) + { + if (size.X < 0.01f) + size.X = 0.01f; + if (size.Y < 0.01f) + size.Y = 0.01f; + if (size.Z < 0.01f) + size.Z = 0.01f; + + strAvatarSize st = new strAvatarSize(); + st.size = size; + st.offset = feetOffset; + AddChange(changes.AvatarSize, st); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character"); + } + + } /// /// This creates the Avatar's physical Surrogate at the position supplied /// @@ -673,7 +694,8 @@ namespace OpenSim.Region.Physics.OdePlugin // sizes one day should came from visual parameters float sx = m_size.X; float sy = m_size.Y; - float sz = m_size.Z + sizeZAdjust; + float sz = m_size.Z; + float topsx = sx * 0.9f; float midsx = sx; @@ -693,7 +715,7 @@ namespace OpenSim.Region.Physics.OdePlugin float midsz = sz - topsz - feetsz; float bonesz = sz; - float bot = -sz * 0.5f; + float bot = -sz * 0.5f + m_feetOffset; boneOff = bot + 0.3f; @@ -754,6 +776,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetOffsetPosition(feetbox, 0, 0, feetz); d.GeomSetOffsetPosition(midbox, 0, 0, midz); d.GeomSetOffsetPosition(topbox, 0, 0, topz); + d.GeomSetOffsetPosition(bonebox, 0, 0, m_feetOffset); // The purpose of the AMotor here is to keep the avatar's physical // surrogate from rotating while moving @@ -1402,6 +1425,12 @@ namespace OpenSim.Region.Physics.OdePlugin { } + private void changeAvatarSize(strAvatarSize st) + { + m_feetOffset = st.offset; + changeSize(st.size); + } + private void changeSize(Vector3 pSize) { if (pSize.IsFinite()) @@ -1609,6 +1638,10 @@ namespace OpenSim.Region.Physics.OdePlugin changeSize((Vector3)arg); break; + case changes.AvatarSize: + changeAvatarSize((strAvatarSize)arg); + break; + case changes.Momentum: changeMomentum((Vector3)arg); break; @@ -1656,5 +1689,12 @@ namespace OpenSim.Region.Physics.OdePlugin { _parent_scene.AddChange((PhysicsActor)this, what, arg); } + + private struct strAvatarSize + { + public Vector3 size; + public float offset; + } + } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 2b4d368..7d1d2fe 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -147,6 +147,7 @@ namespace OpenSim.Region.Physics.OdePlugin PIDHoverActive, Size, + AvatarSize, Shape, PhysRepData, AddPhysRep, -- cgit v1.1 From 2ea0dc55d7ea6da6e91ab614856cdeece7eae5d2 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 7 Dec 2012 20:06:35 +0000 Subject: create a new ode character also with the new information --- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 18 ++++++++++++++++-- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 ++- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 4 ++-- 3 files changed, 20 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index ce269fa..c07213e 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -127,8 +127,10 @@ namespace OpenSim.Region.Physics.Manager /// /// /// - public abstract PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying); - + public virtual PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) + { + return null; + } /// /// Add an avatar /// @@ -145,6 +147,18 @@ namespace OpenSim.Region.Physics.Manager return ret; } + public virtual PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size,float feetOffset, bool isFlying) + { + return null; + } + + public virtual PhysicsActor AddAvatar(uint localID,string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) + { + PhysicsActor ret = AddAvatar(avName, position, size,feetOffset, isFlying); + if (ret != null) ret.LocalID = localID; + return ret; + } + /// /// Remove an avatar. /// diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 9c245e6..15bdc57 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -156,7 +156,7 @@ namespace OpenSim.Region.Physics.OdePlugin - public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float density, float walk_divisor, float rundivisor) + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) { m_uuid = UUID.Random(); @@ -192,6 +192,7 @@ namespace OpenSim.Region.Physics.OdePlugin if(m_size.Z <0.01f) m_size.Z = 0.01f; + m_feetOffset = pfeetOffset; m_orientation = Quaternion.Identity; m_density = density; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 7d1d2fe..2ba5940 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1388,13 +1388,13 @@ namespace OpenSim.Region.Physics.OdePlugin #region Add/Remove Entities - public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) { Vector3 pos; pos.X = position.X; pos.Y = position.Y; pos.Z = position.Z; - OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun); newAv.Flying = isFlying; newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; -- cgit v1.1 From 80639ace95089414423b16f90c8994f50e010373 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 11 Dec 2012 04:36:27 +0000 Subject: a few more changes on avatar collider --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 347 ++++++++++++++++----- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 315 ++++++------------- 2 files changed, 363 insertions(+), 299 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 15bdc57..1b25faf 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -74,7 +74,6 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 _position; private Vector3 _zeroPosition; - private bool _zeroFlag = false; private Vector3 _velocity; private Vector3 _target_velocity; private Vector3 _acceleration; @@ -90,11 +89,15 @@ namespace OpenSim.Region.Physics.OdePlugin public float PID_D; public float PID_P; + private float timeStep; + private float invtimeStep; + private float m_feetOffset = 0; private float feetOff = 0; private float feetSZ = 0.5f; const float feetScale = 0.8f; private float boneOff = 0; + private float m_lastVelocitySqr = 0; public float walkDivisor = 1.3f; public float runDivisor = 0.8f; @@ -103,6 +106,9 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_iscollidingGround = false; private bool m_iscollidingObj = false; private bool m_alwaysRun = false; + + private bool _zeroFlag = false; + private int m_requestedUpdateFrequency = 0; private uint m_localID = 0; public bool m_returnCollisions = false; @@ -120,6 +126,7 @@ namespace OpenSim.Region.Physics.OdePlugin int m_colliderfilter = 0; int m_colliderGroundfilter = 0; int m_colliderObjectfilter = 0; + bool m_collisionException = false; // Default we're a Character private CollisionCategories m_collisionCategories = (CollisionCategories.Character); @@ -132,10 +139,11 @@ namespace OpenSim.Region.Physics.OdePlugin // we do land collisions not ode | CollisionCategories.Land); public IntPtr Body = IntPtr.Zero; private OdeScene _parent_scene; - public IntPtr topbox = IntPtr.Zero; - public IntPtr midbox = IntPtr.Zero; - public IntPtr feetbox = IntPtr.Zero; - public IntPtr bonebox = IntPtr.Zero; + private IntPtr topbox = IntPtr.Zero; + private IntPtr midbox = IntPtr.Zero; + private IntPtr feetbox = IntPtr.Zero; + private IntPtr bbox = IntPtr.Zero; + public IntPtr collider = IntPtr.Zero; public IntPtr Amotor = IntPtr.Zero; @@ -143,6 +151,7 @@ namespace OpenSim.Region.Physics.OdePlugin + public int m_eventsubscription = 0; private int m_cureventsubscription = 0; private CollisionEventUpdate CollisionEventsThisFrame = null; @@ -160,6 +169,9 @@ namespace OpenSim.Region.Physics.OdePlugin { m_uuid = UUID.Random(); + timeStep = parent_scene.ODE_STEPSIZE; + invtimeStep = 1 / timeStep; + if (pos.IsFinite()) { if (pos.Z > 99999f) @@ -208,8 +220,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default - PID_D = basePID_D * m_mass / parent_scene.ODE_STEPSIZE; - PID_P = basePID_P * m_mass / parent_scene.ODE_STEPSIZE; + PID_D = basePID_D * m_mass * invtimeStep; + PID_P = basePID_P * m_mass * invtimeStep; m_isPhysical = false; // current status: no ODE information exists @@ -292,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin set { flying = value; - // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); +// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); } } @@ -336,25 +348,25 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_iscollidingGround; } set { - /* we now control this - if (value) - { - m_colliderGroundfilter += 2; - if (m_colliderGroundfilter > 2) - m_colliderGroundfilter = 2; - } - else - { - m_colliderGroundfilter--; - if (m_colliderGroundfilter < 0) - m_colliderGroundfilter = 0; - } - - if (m_colliderGroundfilter == 0) - m_iscollidingGround = false; - else - m_iscollidingGround = true; - */ +/* we now control this + if (value) + { + m_colliderGroundfilter += 2; + if (m_colliderGroundfilter > 2) + m_colliderGroundfilter = 2; + } + else + { + m_colliderGroundfilter--; + if (m_colliderGroundfilter < 0) + m_colliderGroundfilter = 0; + } + + if (m_colliderGroundfilter == 0) + m_iscollidingGround = false; + else + m_iscollidingGround = true; + */ } } @@ -386,7 +398,7 @@ namespace OpenSim.Region.Physics.OdePlugin else m_iscollidingObj = true; - // m_iscollidingObj = value; +// m_iscollidingObj = value; if (m_iscollidingObj) m_pidControllerActive = false; @@ -634,8 +646,8 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_orientation; } set { - // fakeori = value; - // givefakeori++; +// fakeori = value; +// givefakeori++; value.Normalize(); AddChange(changes.Orientation, value); @@ -690,6 +702,46 @@ namespace OpenSim.Region.Physics.OdePlugin AddChange(changes.Momentum, momentum); } + private void ajustCollider() + { + float vq = _velocity.LengthSquared(); + if (m_lastVelocitySqr != vq) + { + m_lastVelocitySqr = vq; + if (vq > 100.0f) + { + Vector3 off = _velocity; + float t = 0.5f * timeStep; + off = off * t; + d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z); + off.X = 2.0f * (m_size.X + Math.Abs(off.X)); + off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y)); + off.Z = m_size.Z + 2.0f * Math.Abs(off.Z); + d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z); + + d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(bbox, (uint)m_collisionFlags); + d.GeomSetCategoryBits(topbox, 0); + d.GeomSetCollideBits(topbox, 0); + d.GeomSetCategoryBits(midbox, 0); + d.GeomSetCollideBits(midbox, 0); + d.GeomSetCategoryBits(feetbox, 0); + d.GeomSetCollideBits(feetbox, 0); + } + else + { + d.GeomSetCategoryBits(bbox, 0); + d.GeomSetCollideBits(bbox, 0); + d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); + d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); + d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); + d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); + } + } + } + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) { // sizes one day should came from visual parameters @@ -697,7 +749,6 @@ namespace OpenSim.Region.Physics.OdePlugin float sy = m_size.Y; float sz = m_size.Z; - float topsx = sx * 0.9f; float midsx = sx; float feetsx = sx * feetScale; @@ -732,21 +783,17 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); - feetbox = d.CreateBox(_parent_scene.CharsSpace, feetsx, feetsy, feetsz); - d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); - - midbox = d.CreateBox(_parent_scene.CharsSpace, midsx, midsy, midsz); - d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); + collider = d.HashSpaceCreate(_parent_scene.CharsSpace); + d.HashSpaceSetLevels(collider, -4, 3); + d.SpaceSetSublevel(collider, 3); + d.SpaceSetCleanup(collider, false); + d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); + d.GeomSetCollideBits(collider, (uint)m_collisionFlags); - topbox = d.CreateBox(_parent_scene.CharsSpace, topsx, topsy, topsz); - d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); - - bonebox = d.CreateBox(_parent_scene.CharsSpace, bonesx, bonesy, bonesz); - d.GeomSetCategoryBits(bonebox, (uint)m_collisionCategories); - d.GeomSetCollideBits(bonebox, (uint)m_collisionFlags); + feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz); + midbox = d.CreateBox(collider, midsx, midsy, midsz); + topbox = d.CreateBox(collider, topsx, topsy, topsz); + bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z); m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass @@ -758,9 +805,13 @@ namespace OpenSim.Region.Physics.OdePlugin Body = d.BodyCreate(_parent_scene.world); _zeroFlag = false; + m_collisionException = false; m_pidControllerActive = true; m_freemove = false; + _velocity = Vector3.Zero; + m_lastVelocitySqr = 0; + d.BodySetAutoDisableFlag(Body, false); d.BodySetPosition(Body, npositionX, npositionY, npositionZ); @@ -772,12 +823,14 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetBody(feetbox, Body); d.GeomSetBody(midbox, Body); d.GeomSetBody(topbox, Body); - d.GeomSetBody(bonebox, Body); + d.GeomSetBody(bbox, Body); d.GeomSetOffsetPosition(feetbox, 0, 0, feetz); d.GeomSetOffsetPosition(midbox, 0, 0, midz); d.GeomSetOffsetPosition(topbox, 0, 0, topz); - d.GeomSetOffsetPosition(bonebox, 0, 0, m_feetOffset); + + ajustCollider(); + // The purpose of the AMotor here is to keep the avatar's physical // surrogate from rotating while moving @@ -841,44 +894,109 @@ namespace OpenSim.Region.Physics.OdePlugin if (topbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(topbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); + _parent_scene.waitForSpaceUnlock(collider); d.GeomDestroy(topbox); topbox = IntPtr.Zero; } if (midbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(midbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); + _parent_scene.waitForSpaceUnlock(collider); d.GeomDestroy(midbox); midbox = IntPtr.Zero; } if (feetbox != IntPtr.Zero) { _parent_scene.actor_name_map.Remove(feetbox); - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); + _parent_scene.waitForSpaceUnlock(collider); d.GeomDestroy(feetbox); feetbox = IntPtr.Zero; } - if (bonebox != IntPtr.Zero) + if (bbox != IntPtr.Zero) + { + _parent_scene.actor_name_map.Remove(bbox); + _parent_scene.waitForSpaceUnlock(collider); + d.GeomDestroy(bbox); + bbox = IntPtr.Zero; + } + + if (collider != IntPtr.Zero) { - _parent_scene.actor_name_map.Remove(bonebox); - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); - d.GeomDestroy(bonebox); - bonebox = IntPtr.Zero; + d.SpaceDestroy(collider); + collider = IntPtr.Zero; } } + //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z + public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) + { + float sin = 2.0f * rot.Z * rot.W; + float cos = rot.W * rot.W - rot.Z * rot.Z; + float tx = x; + + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) + { + float tx = x; + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) + { + float tx = x; + x = tx * cos + y * sin; + y = -tx * sin + y * cos; + } + + public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) + { + float sin = - 2.0f * rot.Z * rot.W; + float cos = rot.W * rot.W - rot.Z * rot.Z; + float tx = x; + + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + + public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) { feetcollision = false; + if (m_collisionException) + return false; - if (me == bonebox) // inner bone + if (me == bbox) // if moving fast { - if (contact.pos.Z - _position.Z < boneOff) - IsColliding = true; - return true; + // force a full inelastic collision + m_collisionException = true; + + Vector3 off = m_size * 0.5f; + off.X += contact.depth; + off.Y += contact.depth; + off.Z += contact.depth; + if (reverse) + { + off.X *= -contact.normal.X; + off.Y *= -contact.normal.Y; + off.Z *= -contact.normal.Z; + } + else + { + off.X *= contact.normal.X; + off.Y *= contact.normal.Y; + off.Z *= contact.normal.Z; + } + + off.X += contact.pos.X; + off.Y += contact.pos.Y; + off.Z += contact.pos.Z; + + _position = off; + return false; } if (me == topbox) // keep a box head @@ -890,6 +1008,21 @@ namespace OpenSim.Region.Physics.OdePlugin if (me == midbox) { + if (Math.Abs(contact.normal.Z) > 0.95f) + { + float nz = contact.normal.Z; + if (!reverse) + nz = -nz; + + if (nz > 0) + return true; // missed head TODO + + // missed feet collision? + + + return true; + } + t = offx * offx + offy * offy; t = (float)Math.Sqrt(t); t = 1 / t; @@ -917,12 +1050,19 @@ namespace OpenSim.Region.Physics.OdePlugin if (Math.Abs(contact.normal.Z) > 0.95f) { + if (contact.normal.Z > 0) + contact.normal.Z = 1.0f; + else + contact.normal.Z = -1.0f; + contact.normal.X = 0.0f; + contact.normal.Y = 0.0f; feetcollision = true; if (h < boneOff) IsColliding = true; return true; } + float offz = h - feetOff; // distance from top of feetbox if (offz > 0) @@ -971,11 +1111,28 @@ namespace OpenSim.Region.Physics.OdePlugin /// This is the avatar's movement control + PID Controller /// /// - public void Move(float timeStep, List defects) + public void Move(List defects) { if (Body == IntPtr.Zero) return; + if (m_collisionException) + { + d.BodySetPosition(Body,_position.X, _position.Y, _position.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + + float v = _velocity.Length(); + if (v != 0) + { + v = 6.0f / v; + _velocity = _velocity * v; + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } + ajustCollider(); + m_collisionException = false; + return; + } + d.Vector3 dtmp = d.BodyGetPosition(Body); Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); @@ -1049,6 +1206,7 @@ namespace OpenSim.Region.Physics.OdePlugin //****************************************** // colide with land + d.AABB aabb; d.GeomGetAABB(feetbox, out aabb); float chrminZ = aabb.MinZ - 0.02f; // move up a bit @@ -1095,8 +1253,8 @@ namespace OpenSim.Region.Physics.OdePlugin contact.Position.X = localpos.X; contact.Position.Y = localpos.Y; contact.Position.Z = chrminZ; - contact.SurfaceNormal.X = 0f; - contact.SurfaceNormal.Y = 0f; + contact.SurfaceNormal.X = 0.0f; + contact.SurfaceNormal.Y = 0.0f; contact.SurfaceNormal.Z = -1f; contact.RelativeSpeed = -vel.Z; contact.CharacterFeet = true; @@ -1118,6 +1276,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_iscollidingGround = false; } + //****************************************** bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); @@ -1253,21 +1412,58 @@ namespace OpenSim.Region.Physics.OdePlugin } // update our local ideia of position velocity and aceleration + // _position = localpos; _position = localpos; + if (_zeroFlag) { _velocity = Vector3.Zero; _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; } else { - _acceleration = _velocity; // previus velocity - _velocity = vel; - _acceleration = (vel - _acceleration) / timeStep; + Vector3 a =_velocity; // previus velocity + SetSmooth(ref _velocity, ref vel, 2); + a = (_velocity - a) * invtimeStep; + SetSmooth(ref _acceleration, ref a, 2); + + dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = 0f; + m_rotationalVelocity.Y = 0f; + m_rotationalVelocity.Z = dtmp.Z; + Math.Round(m_rotationalVelocity.Z,3); } - + ajustCollider(); } + public void round(ref Vector3 v, int digits) + { + v.X = (float)Math.Round(v.X, digits); + v.Y = (float)Math.Round(v.Y, digits); + v.Z = (float)Math.Round(v.Z, digits); + } + + 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; + } + + public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) + { + dst.X = 0.4f * dst.X + 0.6f * value.X; + dst.X = (float)Math.Round(dst.X, rounddigits); + + dst.Y = 0.4f * dst.Y + 0.6f * value.Y; + dst.Y = (float)Math.Round(dst.Y, rounddigits); + + dst.Z = 0.4f * dst.Z + 0.6f * value.Z; + 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 @@ -1394,10 +1590,11 @@ namespace OpenSim.Region.Physics.OdePlugin AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); - _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[collider] = (PhysicsActor)this; _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this; + _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; _parent_scene.AddCharacter(this); } else @@ -1450,13 +1647,16 @@ namespace OpenSim.Region.Physics.OdePlugin _position.Z + (m_size.Z - oldsz) * 0.5f); Velocity = Vector3.Zero; + - _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[collider] = (PhysicsActor)this; _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this; + _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; } m_freemove = false; + m_collisionException = false; m_pidControllerActive = true; } else @@ -1565,6 +1765,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); + ajustCollider(); } private void donullchange() @@ -1573,7 +1774,7 @@ namespace OpenSim.Region.Physics.OdePlugin public bool DoAChange(changes what, object arg) { - if (topbox == IntPtr.Zero && what != changes.Add && what != changes.Remove) + if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove) { return false; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 2ba5940..f8d7195 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -178,7 +178,9 @@ namespace OpenSim.Region.Physics.OdePlugin public changes what; public Object arg; } - + + + public class OdeScene : PhysicsScene { private readonly ILog m_log; @@ -301,6 +303,8 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr StaticSpace; // space for the static things around public IntPtr GroundSpace; // space for ground + public IntPtr SharedRay; + // some speedup variables private int spaceGridMaxX; private int spaceGridMaxY; @@ -428,6 +432,8 @@ namespace OpenSim.Region.Physics.OdePlugin contactgroup = d.JointGroupCreate(0); //contactgroup + SharedRay = d.CreateRay(TopSpace, 1.0f); + d.WorldSetAutoDisableFlag(world, false); } } @@ -733,35 +739,35 @@ namespace OpenSim.Region.Physics.OdePlugin if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; -/* -// debug - PhysicsActor dp2; - if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) - { - d.AABB aabb; - d.GeomGetAABB(g2, out aabb); - float x = aabb.MaxX - aabb.MinX; - float y = aabb.MaxY - aabb.MinY; - float z = aabb.MaxZ - aabb.MinZ; - if (x > 60.0f || y > 60.0f || z > 60.0f) - { - if (!actor_name_map.TryGetValue(g2, out dp2)) - m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); - else - m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", - dp2.Name, dp2.Size, x, y, z, - dp2.Position.ToString(), - dp2.Orientation.ToString(), - dp2.Orientation.Length()); - return; - } - } -// -*/ + /* + // debug + PhysicsActor dp2; + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + { + d.AABB aabb; + d.GeomGetAABB(g2, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if (x > 60.0f || y > 60.0f || z > 60.0f) + { + if (!actor_name_map.TryGetValue(g2, out dp2)) + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + else + m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", + dp2.Name, dp2.Size, x, y, z, + dp2.Position.ToString(), + dp2.Orientation.ToString(), + dp2.Orientation.Length()); + return; + } + } + // + */ - if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || - d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) + if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || + d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) { int cflags; unchecked @@ -776,7 +782,7 @@ namespace OpenSim.Region.Physics.OdePlugin 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); + // ode.drelease(world); base.TriggerPhysicsBasedRestart(); } catch (Exception e) @@ -816,26 +822,25 @@ namespace OpenSim.Region.Physics.OdePlugin // get first contact d.ContactGeom curContact = new d.ContactGeom(); + if (!GetCurContactGeom(0, ref curContact)) return; - // for now it's the one with max depth - ContactPoint maxDepthContact = new ContactPoint( - new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), - new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), - curContact.depth - ); + // do volume detection case - if ( - (p1.IsVolumeDtc || p2.IsVolumeDtc)) + if ((p1.IsVolumeDtc || p2.IsVolumeDtc)) { + ContactPoint maxDepthContact = new ContactPoint( + new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), + new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), + curContact.depth, false + ); + collision_accounting_events(p1, p2, maxDepthContact); return; } // big messy collision analises - Vector3 normoverride = Vector3.Zero; //damm c# - float mu = 0; float bounce = 0; float cfm = 0.0001f; @@ -846,36 +851,15 @@ namespace OpenSim.Region.Physics.OdePlugin ContactData contactdata1 = new ContactData(0, 0, false); ContactData contactdata2 = new ContactData(0, 0, false); - bool dop1foot = false; - bool dop2foot = false; + bool dop1ava = false; + bool dop2ava = false; bool ignore = false; - bool AvanormOverride = false; switch (p1.PhysicsActorType) { case (int)ActorTypes.Agent: { - dop1foot = true; - - AvanormOverride = true; - Vector3 tmp = p2.Position - p1.Position; - normoverride = p2.Velocity - p1.Velocity; - mu = normoverride.LengthSquared(); - - if (mu > 1e-6) - { - mu = 1.0f / (float)Math.Sqrt(mu); - normoverride *= mu; - mu = Vector3.Dot(tmp, normoverride); - if (mu > 0) - normoverride *= -1; - } - else - { - tmp.Normalize(); - normoverride = -tmp; - } - + dop1ava = true; switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: @@ -886,7 +870,6 @@ namespace OpenSim.Region.Physics.OdePlugin case (int)ActorTypes.Prim: if (p2.Velocity.LengthSquared() > 0.0f) p2.CollidingObj = true; - dop1foot = true; break; default: @@ -901,33 +884,8 @@ namespace OpenSim.Region.Physics.OdePlugin { case (int)ActorTypes.Agent: + dop2ava = true; - dop2foot = true; - - AvanormOverride = true; - - Vector3 tmp = p2.Position - p1.Position; - normoverride = p2.Velocity - p1.Velocity; - mu = normoverride.LengthSquared(); - if (mu > 1e-6) - { - mu = 1.0f / (float)Math.Sqrt(mu); - normoverride *= mu; - mu = Vector3.Dot(tmp, normoverride); - if (mu > 0) - normoverride *= -1; - } - else - { - tmp.Normalize(); - normoverride = -tmp; - } - - bounce = 0; - mu = 0; - cfm = 0.0001f; - - dop2foot = true; if (p1.Velocity.LengthSquared() > 0.0f) p1.CollidingObj = true; break; @@ -1032,170 +990,78 @@ namespace OpenSim.Region.Physics.OdePlugin default: break; } + if (ignore) return; - IntPtr Joint; - bool FeetCollision = false; + d.ContactGeom maxContact = curContact; +// if (IgnoreNegSides && curContact.side1 < 0) +// maxContact.depth = float.MinValue; + + d.ContactGeom minContact = curContact; +// if (IgnoreNegSides && curContact.side1 < 0) +// minContact.depth = float.MaxValue; - int i = 0; + IntPtr Joint; + bool FeetCollision = false; int ncontacts = 0; - while(true) - { - if (IgnoreNegSides && curContact.side1 < 0) - { - if (++i >= count) - break; - if (!GetCurContactGeom(i, ref curContact)) - break; - } - else + int i = 0; + while (true) { - if(dop1foot) + if (m_global_contactcount >= maxContactsbeforedeath) + break; + +// if (!(IgnoreNegSides && curContact.side1 < 0)) { - if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision))) + bool noskip = true; + if (dop1ava) { - if (++i >= count) - break; - else - continue; + if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact, ref FeetCollision))) + + noskip = false; } - } - else if(dop2foot) - { - if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision))) + else if (dop2ava) { - if (++i >= count) - break; - else - continue; + if (!(((OdeCharacter)p2).Collide(g2,true, ref curContact, ref FeetCollision))) + noskip = false; } - } -/* - if (AvanormOverride) - { - if (curContact.depth > 0.3f) + if (noskip) { - if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) - p1.IsColliding = true; - if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) - p2.IsColliding = true; - curContact.normal.X = normoverride.X; - curContact.normal.Y = normoverride.Y; - curContact.normal.Z = normoverride.Z; - } + m_global_contactcount++; + ncontacts++; - else - { - if (dop1foot) - { - float sz = p1.Size.Z; - Vector3 vtmp = p1.Position; - float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; - if (ppos > 0f) - { - if (!p1.Flying) - { - d.AABB aabb; - d.GeomGetAABB(g2, out aabb); - float tmp = vtmp.Z - sz * .18f; - - if (aabb.MaxZ < tmp) - { - vtmp.X = curContact.pos.X - vtmp.X; - vtmp.Y = curContact.pos.Y - vtmp.Y; - vtmp.Z = -0.2f; - vtmp.Normalize(); - curContact.normal.X = vtmp.X; - curContact.normal.Y = vtmp.Y; - curContact.normal.Z = vtmp.Z; - } - } - } - else - p1.IsColliding = true; + Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); + d.JointAttach(Joint, b1, b2); - } + if (curContact.depth > maxContact.depth) + maxContact = curContact; - if (dop2foot) - { - float sz = p2.Size.Z; - Vector3 vtmp = p2.Position; - vtmp.Z -= sz * 0.5f; - vtmp.Z += 0.5f; - float ppos = vtmp.Z - curContact.pos.Z; - if (ppos > 0f) - { - if (!p2.Flying) - { - float tmp = vtmp.Z - sz * .18f; - vtmp.X = curContact.pos.X - vtmp.X; - vtmp.Y = curContact.pos.Y - vtmp.Y; - vtmp.Z = curContact.pos.Z - vtmp.Z; - vtmp.Normalize(); - curContact.normal.X = vtmp.X; - curContact.normal.Y = vtmp.Y; - curContact.normal.Z = vtmp.Z; - } - } -// else - p2.IsColliding = true; - - } + if (curContact.depth < minContact.depth) + minContact = curContact; } } -*/ - ncontacts++; - Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); - d.JointAttach(Joint, b1, b2); - - if (++m_global_contactcount >= maxContactsbeforedeath) - break; if (++i >= count) break; if (!GetCurContactGeom(i, ref curContact)) break; - - if (curContact.depth > maxDepthContact.PenetrationDepth) - { - maxDepthContact.Position.X = curContact.pos.X; - maxDepthContact.Position.Y = curContact.pos.Y; - maxDepthContact.Position.Z = curContact.pos.Z; - maxDepthContact.SurfaceNormal.X = curContact.normal.X; - maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; - maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; - maxDepthContact.PenetrationDepth = curContact.depth; - } } - } if (ncontacts > 0) { - maxDepthContact.CharacterFeet = FeetCollision; + ContactPoint maxDepthContact = new ContactPoint( + new Vector3(maxContact.pos.X, maxContact.pos.Y, maxContact.pos.Z), + new Vector3(minContact.normal.X, minContact.normal.Y, minContact.normal.Z), + maxContact.depth, FeetCollision + ); collision_accounting_events(p1, p2, maxDepthContact); } -/* - if (notskipedcount > geomContactPointsStartthrottle) - { - // If there are more then 3 contact points, it's likely - // that we've got a pile of objects, so ... - // We don't want to send out hundreds of terse updates over and over again - // so lets throttle them and send them again after it's somewhat sorted out. - this needs checking so out for now - if (b1 != IntPtr.Zero) - p1.ThrottleUpdates = true; - if (b2 != IntPtr.Zero) - p2.ThrottleUpdates = true; - - } - */ } private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) @@ -1286,10 +1152,7 @@ namespace OpenSim.Region.Physics.OdePlugin // chr.CollidingGround = false; not done here chr.CollidingObj = false; // do colisions with static space - d.SpaceCollide2(StaticSpace, chr.topbox, IntPtr.Zero, nearCallback); - d.SpaceCollide2(StaticSpace, chr.midbox, IntPtr.Zero, nearCallback); - d.SpaceCollide2(StaticSpace, chr.feetbox, IntPtr.Zero, nearCallback); - d.SpaceCollide2(StaticSpace, chr.bonebox, IntPtr.Zero, nearCallback); + d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback); // chars with chars d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); @@ -1346,7 +1209,7 @@ namespace OpenSim.Region.Physics.OdePlugin // and with chars try { - d.SpaceCollide2(ActiveSpace, CharsSpace,IntPtr.Zero, nearCallback); + d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback); } catch (AccessViolationException) { @@ -1837,7 +1700,7 @@ namespace OpenSim.Region.Physics.OdePlugin foreach (OdeCharacter actor in _characters) { if (actor != null) - actor.Move(ODE_STEPSIZE, defects); + actor.Move(defects); } if (defects.Count != 0) { -- cgit v1.1 From 420015b7d0dafe956d7ab1980d8e4f499395df76 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 11 Dec 2012 04:47:40 +0000 Subject: missing file --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 0405dad..e39cee7 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -71,6 +71,15 @@ namespace OpenSim.Region.Physics.Manager RelativeSpeed = 0f; // for now let this one be set explicity CharacterFeet = true; // keep other plugins work as before } + + public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth, bool feet) + { + Position = position; + SurfaceNormal = surfaceNormal; + PenetrationDepth = penetrationDepth; + RelativeSpeed = 0f; // for now let this one be set explicity + CharacterFeet = feet; // keep other plugins work as before + } } public struct ContactData -- cgit v1.1 From 9a253c7c04281b9f9a13dda1c098251b1d81a70a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 11 Dec 2012 04:49:14 +0000 Subject: missing files remove some warnings... --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 4 ++-- OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs | 1 - OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 7dddab6..faa9488 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2521,7 +2521,7 @@ namespace OpenSim.Region.Physics.OdePlugin prm.m_collisionCategories = m_collisionCategories; prm.m_collisionFlags = m_collisionFlags; - if (prm.prim_geom != null) + if (prm.prim_geom != IntPtr.Zero) { if (prm.m_NoColide) @@ -2542,7 +2542,7 @@ namespace OpenSim.Region.Physics.OdePlugin // ((OdePrim)_parent).ChildSelectedChange(true); - if (prim_geom != null) + if (prim_geom != IntPtr.Zero) { if (m_NoColide) { diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index f449099..561ab1c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -555,7 +555,6 @@ namespace OpenSim.Region.Physics.OdePlugin break; default: - return; break; } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index f8d7195..8878820 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -515,7 +515,7 @@ namespace OpenSim.Region.Physics.OdePlugin odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); - GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); + GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); m_materialContactsData[(int)Material.Stone].mu = 0.8f; m_materialContactsData[(int)Material.Stone].bounce = 0.4f; -- cgit v1.1 From 28ea08c3e234f8ca3c6590af4045349af7fed844 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 11 Dec 2012 17:14:32 +0000 Subject: fix let other phys plugins work.. broken when added feetOffset --- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 10 ++-------- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 7 ++++--- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 4 ++-- 3 files changed, 8 insertions(+), 13 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index c07213e..cdffa6b 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -147,15 +147,9 @@ namespace OpenSim.Region.Physics.Manager return ret; } - public virtual PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size,float feetOffset, bool isFlying) + public virtual PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) { - return null; - } - - public virtual PhysicsActor AddAvatar(uint localID,string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) - { - PhysicsActor ret = AddAvatar(avName, position, size,feetOffset, isFlying); - if (ret != null) ret.LocalID = localID; + PhysicsActor ret = AddAvatar(localID, avName, position, size, isFlying); return ret; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 1b25faf..b769c88 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -165,9 +165,10 @@ namespace OpenSim.Region.Physics.OdePlugin - public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) + public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) { m_uuid = UUID.Random(); + m_localID = localID; timeStep = parent_scene.ODE_STEPSIZE; invtimeStep = 1 / timeStep; @@ -1209,7 +1210,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.AABB aabb; d.GeomGetAABB(feetbox, out aabb); - float chrminZ = aabb.MinZ - 0.02f; // move up a bit + float chrminZ = aabb.MinZ; ; // move up a bit Vector3 posch = localpos; float ftmp; @@ -1252,7 +1253,7 @@ namespace OpenSim.Region.Physics.OdePlugin contact.PenetrationDepth = depth; contact.Position.X = localpos.X; contact.Position.Y = localpos.Y; - contact.Position.Z = chrminZ; + contact.Position.Z = terrainheight; contact.SurfaceNormal.X = 0.0f; contact.SurfaceNormal.Y = 0.0f; contact.SurfaceNormal.Z = -1f; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 8878820..15eb01f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1251,13 +1251,13 @@ namespace OpenSim.Region.Physics.OdePlugin #region Add/Remove Entities - public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) + public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) { Vector3 pos; pos.X = position.X; pos.Y = position.Y; pos.Z = position.Z; - OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + OdeCharacter newAv = new OdeCharacter(localID,avName, this, pos, size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun); newAv.Flying = isFlying; newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; -- cgit v1.1 From f35e3c6fe04327ad4bc9b9864663910ea2b5d717 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 11 Dec 2012 19:38:44 +0000 Subject: changes on the fast speed avatars collider, collisions from above, etc --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 126 +++++++++++---------- 1 file changed, 64 insertions(+), 62 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b769c88..27a9f1c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -970,78 +970,86 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_collisionException) return false; + Vector3 offset; + if (me == bbox) // if moving fast { // force a full inelastic collision m_collisionException = true; - Vector3 off = m_size * 0.5f; - off.X += contact.depth; - off.Y += contact.depth; - off.Z += contact.depth; + offset = m_size * m_orientation; + + offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth; + offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth; + offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth; + if (reverse) { - off.X *= -contact.normal.X; - off.Y *= -contact.normal.Y; - off.Z *= -contact.normal.Z; + offset.X *= -contact.normal.X; + offset.Y *= -contact.normal.Y; + offset.Z *= -contact.normal.Z; } else { - off.X *= contact.normal.X; - off.Y *= contact.normal.Y; - off.Z *= contact.normal.Z; + offset.X *= contact.normal.X; + offset.Y *= contact.normal.Y; + offset.Z *= contact.normal.Z; } - off.X += contact.pos.X; - off.Y += contact.pos.Y; - off.Z += contact.pos.Z; + offset.X += contact.pos.X; + offset.Y += contact.pos.Y; + offset.Z += contact.pos.Z; - _position = off; + _position = offset; return false; } - if (me == topbox) // keep a box head - return true; - - float t; - float offx = contact.pos.X - _position.X; - float offy = contact.pos.Y - _position.Y; + offset.X = contact.pos.X - _position.X; + offset.Y = contact.pos.Y - _position.Y; - if (me == midbox) + if (me == topbox) { - if (Math.Abs(contact.normal.Z) > 0.95f) - { - float nz = contact.normal.Z; - if (!reverse) - nz = -nz; - - if (nz > 0) - return true; // missed head TODO + offset.Z = contact.pos.Z - _position.Z; - // missed feet collision? + offset.Normalize(); - - return true; + if (reverse) + { + contact.normal.X = offset.X; + contact.normal.Y = offset.Y; + contact.normal.Z = offset.Z; + } + else + { + contact.normal.X = -offset.X; + contact.normal.Y = -offset.Y; + contact.normal.Z = -offset.Z; } + return true; + } - t = offx * offx + offy * offy; - t = (float)Math.Sqrt(t); - t = 1 / t; - offx *= t; - offy *= t; + if (me == midbox) + { + if (Math.Abs(contact.normal.Z) > 0.95f) + offset.Z = contact.pos.Z - _position.Z; + else + offset.Z = contact.normal.Z; + + offset.Normalize(); if (reverse) { - contact.normal.X = offx; - contact.normal.Y = offy; + contact.normal.X = offset.X; + contact.normal.Y = offset.Y; + contact.normal.Z = offset.Z; } else { - contact.normal.X = -offx; - contact.normal.Y = -offy; + contact.normal.X = -offset.X; + contact.normal.Y = -offset.Y; + contact.normal.Z = -offset.Z; } - contact.normal.Z = 0; return true; } @@ -1063,39 +1071,33 @@ namespace OpenSim.Region.Physics.OdePlugin return true; } + offset.Z = h - feetOff; // distance from top of feetbox - float offz = h - feetOff; // distance from top of feetbox - - if (offz > 0) + if (offset.Z > 0) return false; - if (offz > -0.01) + if (offset.Z > -0.01) { - offx = 0; - offy = 0; - offz = -1.0f; + offset.X = 0; + offset.Y = 0; + offset.Z = -1.0f; } else { - t = offx * offx + offy * offy + offz * offz; - t = (float)Math.Sqrt(t); - t = 1 / t; - offx *= t; - offy *= t; - offz *= t; + offset.Normalize(); } if (reverse) { - contact.normal.X = offx; - contact.normal.Y = offy; - contact.normal.Z = offz; + contact.normal.X = offset.X; + contact.normal.Y = offset.Y; + contact.normal.Z = offset.Z; } else { - contact.normal.X = -offx; - contact.normal.Y = -offy; - contact.normal.Z = -offz; + contact.normal.X = -offset.X; + contact.normal.Y = -offset.Y; + contact.normal.Z = -offset.Z; } feetcollision = true; if (h < boneOff) @@ -1125,7 +1127,7 @@ namespace OpenSim.Region.Physics.OdePlugin float v = _velocity.Length(); if (v != 0) { - v = 6.0f / v; + v = 5.0f / v; _velocity = _velocity * v; d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); } -- cgit v1.1 From 71fc9f29f92edf2956ecb25dcbcdb97d262c8a14 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 11 Dec 2012 22:53:24 +0000 Subject: make ubitODE ignore X and Y rotation components on avatar rotations --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 51 ++++++++++++++++------ 1 file changed, 37 insertions(+), 14 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 27a9f1c..bb04ea7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -80,6 +80,7 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_rotationalVelocity; private Vector3 m_size; private Quaternion m_orientation; + private Quaternion m_orientation2D; private float m_mass = 80f; public float m_density = 60f; private bool m_pidControllerActive = true; @@ -207,6 +208,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_feetOffset = pfeetOffset; m_orientation = Quaternion.Identity; + m_orientation2D = Quaternion.Identity; m_density = density; // force lower density for testing @@ -649,7 +651,6 @@ namespace OpenSim.Region.Physics.OdePlugin { // fakeori = value; // givefakeori++; - value.Normalize(); AddChange(changes.Orientation, value); } @@ -977,7 +978,7 @@ namespace OpenSim.Region.Physics.OdePlugin // force a full inelastic collision m_collisionException = true; - offset = m_size * m_orientation; + offset = m_size * m_orientation2D; offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth; offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth; @@ -1143,10 +1144,10 @@ namespace OpenSim.Region.Physics.OdePlugin // so force it back to identity d.Quaternion qtmp; - qtmp.W = m_orientation.W; - qtmp.X = m_orientation.X; - qtmp.Y = m_orientation.Y; - qtmp.Z = m_orientation.Z; + qtmp.W = m_orientation2D.W; + qtmp.X = m_orientation2D.X; + qtmp.Y = m_orientation2D.Y; + qtmp.Z = m_orientation2D.Z; d.BodySetQuaternion(Body, ref qtmp); if (m_pidControllerActive == false) @@ -1679,14 +1680,36 @@ namespace OpenSim.Region.Physics.OdePlugin private void changeOrientation(Quaternion newOri) { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - float t = d.JointGetAMotorAngle(Amotor, 2); - d.BodySetQuaternion(Body,ref myrot); - m_orientation = newOri; + if (m_orientation != newOri) + { + m_orientation = newOri; // keep a copy for core use + // but only use rotations around Z + + 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 / (float)Math.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; + + d.Quaternion myrot = new d.Quaternion(); + myrot.X = m_orientation2D.X; + myrot.Y = m_orientation2D.Y; + myrot.Z = m_orientation2D.Z; + myrot.W = m_orientation2D.W; + d.BodySetQuaternion(Body, ref myrot); + } } private void changeVelocity(Vector3 newVel) -- cgit v1.1 From d003aa2e7b690d21f7d6094431c16ad0de5776d4 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 17 Dec 2012 01:40:48 +0000 Subject: * TEST * unscripted sit --- .../Region/Physics/UbitOdePlugin/ODESitAvatar.cs | 183 +++++++++++++++++++++ OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 15 +- 2 files changed, 196 insertions(+), 2 deletions(-) create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs new file mode 100644 index 0000000..225bff8 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -0,0 +1,183 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +// Ubit 2012 +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OdeAPI; +using log4net; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// + public class ODESitAvatar + { + private OdeScene m_scene; + private ODERayCastRequestManager m_raymanager; + + public ODESitAvatar(OdeScene pScene, ODERayCastRequestManager raymanager) + { + m_scene = pScene; + m_raymanager = raymanager; + } + + private static Vector3 SitAjust = new Vector3(0, 0, 0.4f); + + public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) + { + if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero) + { + PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); + return; + } + + IntPtr geom = ((OdePrim)actor).prim_geom; + + d.Vector3 dtmp = d.GeomGetPosition(geom); + Vector3 geopos; + geopos.X = dtmp.X; + geopos.Y = dtmp.Y; + geopos.Z = dtmp.Z; + + + d.AABB aabb; + Quaternion ori; + d.Quaternion qtmp; + d.GeomCopyQuaternion(geom, out qtmp); + Quaternion geomOri; + geomOri.X = qtmp.X; + geomOri.Y = qtmp.Y; + geomOri.Z = qtmp.Z; + geomOri.W = qtmp.W; + Quaternion geomInvOri; + geomInvOri.X = -qtmp.X; + geomInvOri.Y = -qtmp.Y; + geomInvOri.Z = -qtmp.Z; + geomInvOri.W = qtmp.W; + + Vector3 target = geopos + offset; + Vector3 rayDir = target - avCameraPosition; + float raylen = rayDir.Length(); + float t = 1 / raylen; + rayDir.X *= t; + rayDir.Y *= t; + rayDir.Z *= t; + + raylen += 0.5f; + List rayResults; + + rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir , raylen, 1); + if (rayResults.Count == 0 || rayResults[0].ConsumerID != actor.LocalID) + { + d.GeomGetAABB(geom,out aabb); + offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); + ori = geomInvOri; + offset *= geomInvOri; + + PhysicsSitResponse(1, actor.LocalID, offset, ori); + return; + } + + offset = rayResults[0].Pos - geopos; + double ang; + float s; + float c; + + d.GeomClassID geoclass = d.GeomGetClass(geom); + + if (geoclass == d.GeomClassID.SphereClass) + { + float r = d.GeomSphereGetRadius(geom); + + offset.Normalize(); + offset *= r; + + ang = Math.Atan2(offset.Y, offset.X); + ang *= 0.5d; + s = (float)Math.Sin(ang); + c = (float)Math.Cos(ang); + + ori = new Quaternion(0, 0, s, c); + + if (r < 0.4f) + { + offset = new Vector3(0, 0, r); + } + else if (offset.Z < 0.4f) + { + t = offset.Z; + float rsq = r * r; + + t = 1.0f / (rsq - t * t); + offset.X *= t; + offset.Y *= t; + offset.Z = 0.4f; + t = rsq - 0.16f; + offset.X *= t; + offset.Y *= t; + } + + offset += avOffset * ori; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(1, actor.LocalID, offset, ori); + return; + } + + Vector3 norm = rayResults[0].Normal; + + if (norm.Z < 0) + { + PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); + return; + } + + ang = Math.Atan2(-rayDir.Y, -rayDir.X); + ang *= 0.5d; + s = (float)Math.Sin(ang); + c = (float)Math.Cos(ang); + + ori = new Quaternion(0, 0, s, c); + + offset += avOffset * ori; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(1, actor.LocalID, offset, ori); + return; + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 15eb01f..fbf2f0d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -362,8 +362,7 @@ namespace OpenSim.Region.Physics.OdePlugin nearCallback = near; - m_rayCastManager = new ODERayCastRequestManager(this); - + m_rayCastManager = new ODERayCastRequestManager(this); lock (OdeLock) { @@ -2711,5 +2710,17 @@ namespace OpenSim.Region.Physics.OdePlugin } return new List(); } + + public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) + { + Util.FireAndForget( delegate + { + ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager); + if(sitAvatar != null) + sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse); + }); + return 1; + } + } } -- cgit v1.1 From 9d1a6558d9e65ab70703cea15b3752c33c4ee244 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 17 Dec 2012 01:43:16 +0000 Subject: *TESTP unscripted sit: missing files --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 1 + OpenSim/Region/Physics/Manager/PhysicsScene.cs | 5 +++++ 2 files changed, 6 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index e39cee7..9338130 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -411,6 +411,7 @@ namespace OpenSim.Region.Physics.Manager // Warning in a parent part it returns itself, not null public virtual PhysicsActor ParentActor { get { return this; } } + } public class NullPhysicsActor : PhysicsActor diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index cdffa6b..a442cf0 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -38,6 +38,7 @@ namespace OpenSim.Region.Physics.Manager public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal); public delegate void RayCallback(List list); + public delegate void SitAvatarCallback(int status, uint partID, Vector3 offset, Quaternion Orientation); public delegate void JointMoved(PhysicsJoint joint); public delegate void JointDeactivated(PhysicsJoint joint); @@ -357,5 +358,9 @@ namespace OpenSim.Region.Physics.Manager return new List(); } + public virtual int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) + { + return 0; + } } } -- cgit v1.1 From 9089bc7e37879867756740cfbd0456ad51244ca0 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 17 Dec 2012 22:11:29 +0000 Subject: some clean up/fix --- OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs index 225bff8..c1a0ca9 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -93,7 +93,7 @@ namespace OpenSim.Region.Physics.OdePlugin rayDir.Y *= t; rayDir.Z *= t; - raylen += 0.5f; + raylen += 30f; // focal point may be far List rayResults; rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir , raylen, 1); -- cgit v1.1 From f87af983f393526ab6cebfbd711abdacd979a6ff Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 18 Dec 2012 00:50:36 +0000 Subject: fix --- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 2 +- .../Physics/UbitOdePlugin/ODERayCastRequestManager.cs | 14 ++++++++++++++ OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs | 19 ++++++++++++------- 3 files changed, 27 insertions(+), 8 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index a442cf0..86e0713 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -353,7 +353,7 @@ namespace OpenSim.Region.Physics.Manager public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){} public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { } - public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) { return new List(); } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 561ab1c..54a83c2 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -199,6 +199,20 @@ namespace OpenSim.Region.Physics.OdePlugin m_PendingRequests.Enqueue(req); } + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count,RayFilterFlags flags, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + req.filter = flags; + + m_PendingRequests.Enqueue(req); + } + public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) { ODERayRequest req = new ODERayRequest(); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs index c1a0ca9..fd3a3ba 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -52,6 +52,7 @@ namespace OpenSim.Region.Physics.OdePlugin } private static Vector3 SitAjust = new Vector3(0, 0, 0.4f); + private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit; public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) { @@ -96,7 +97,7 @@ namespace OpenSim.Region.Physics.OdePlugin raylen += 30f; // focal point may be far List rayResults; - rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir , raylen, 1); + rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); if (rayResults.Count == 0 || rayResults[0].ConsumerID != actor.LocalID) { d.GeomGetAABB(geom,out aabb); @@ -108,6 +109,7 @@ namespace OpenSim.Region.Physics.OdePlugin return; } + offset = rayResults[0].Pos - geopos; double ang; float s; @@ -156,13 +158,16 @@ namespace OpenSim.Region.Physics.OdePlugin return; } - Vector3 norm = rayResults[0].Normal; +/* + // contact normals aren't reliable on meshs or sculpts it seems + Vector3 norm = rayResults[0].Normal; - if (norm.Z < 0) - { - PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); - return; - } + if (norm.Z < 0) + { + PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); + return; + } +*/ ang = Math.Atan2(-rayDir.Y, -rayDir.X); ang *= 0.5d; -- cgit v1.1 From ed9cb18cf0cb5b76946d7afb79e3c8677110c41b Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 18 Dec 2012 00:54:40 +0000 Subject: missing file --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index fbf2f0d..d344d4d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2675,9 +2675,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); } } - - // don't like this - public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + + public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) { if (actor != null) { @@ -2698,7 +2697,7 @@ namespace OpenSim.Region.Physics.OdePlugin results.CopyTo(ourResults, 0); }; int waitTime = 0; - m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, flags, retMethod); while (ourResults == null && waitTime < 1000) { Thread.Sleep(1); -- cgit v1.1 From ca30559b05bd44995277c270ec19a92d8170e587 Mon Sep 17 00:00:00 2001 From: Melanie Date: Sun, 23 Dec 2012 18:07:32 +0000 Subject: Whitespace change to trigger ircbot --- OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs | 1 + 1 file changed, 1 insertion(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs index 8e059ee..9d9532d 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs @@ -24,6 +24,7 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ + using System; using System.Collections.Generic; using System.Reflection; -- cgit v1.1 From 6eeb4cb3f745b3180220b0ab465cf4925cbdcddb Mon Sep 17 00:00:00 2001 From: Melanie Date: Sun, 23 Dec 2012 18:10:34 +0000 Subject: Revert "Whitespace change to trigger ircbot" This reverts commit ca30559b05bd44995277c270ec19a92d8170e587. --- OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs | 1 - 1 file changed, 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs index 9d9532d..8e059ee 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs @@ -24,7 +24,6 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - using System; using System.Collections.Generic; using System.Reflection; -- cgit v1.1 From 7980a1d8496b71e46eeead7d77382cd10ac9fa78 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 2 Jan 2013 19:39:46 +0000 Subject: *TEST* avatar unscripted sit. Some guessing/automation --- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 21 +- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 13 + .../UbitOdePlugin/ODERayCastRequestManager.cs | 583 +++++++++++++-------- .../Region/Physics/UbitOdePlugin/ODESitAvatar.cs | 260 +++++++-- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 254 ++++++--- 5 files changed, 801 insertions(+), 330 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 86e0713..d24ab2a 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -38,6 +38,9 @@ namespace OpenSim.Region.Physics.Manager public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal); public delegate void RayCallback(List list); + public delegate void ProbeBoxCallback(List list); + public delegate void ProbeSphereCallback(List list); + public delegate void ProbePlaneCallback(List list); public delegate void SitAvatarCallback(int status, uint partID, Vector3 offset, Quaternion Orientation); public delegate void JointMoved(PhysicsJoint joint); @@ -56,6 +59,7 @@ namespace OpenSim.Region.Physics.Manager volumedtc = 0x40, // ray cast colision control (may only work for meshs) + ContactsUnImportant = 0x2000, BackFaceCull = 0x4000, ClosestHit = 0x8000, @@ -351,13 +355,26 @@ namespace OpenSim.Region.Physics.Manager return false; } - public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){} - public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { } public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) { return new List(); } + public virtual List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) + { + return new List(); + } + + public virtual List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) + { + return new List(); + } + + public virtual List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) + { + return new List(); + } + public virtual int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) { return 0; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index bb04ea7..e1d694e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -715,7 +715,17 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 off = _velocity; float t = 0.5f * timeStep; off = off * t; + d.Quaternion qtmp; + d.GeomCopyQuaternion(bbox, out qtmp); + Quaternion q; + q.X = qtmp.X; + q.Y = qtmp.Y; + q.Z = qtmp.Z; + q.W = qtmp.W; + off *= Quaternion.Conjugate(q); + d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z); + off.X = 2.0f * (m_size.X + Math.Abs(off.X)); off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y)); off.Z = m_size.Z + 2.0f * Math.Abs(off.Z); @@ -741,6 +751,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); } + uint cat1 = d.GeomGetCategoryBits(bbox); + uint col1 = d.GeomGetCollideBits(bbox); + } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 54a83c2..31757a9 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -56,8 +56,11 @@ namespace OpenSim.Region.Physics.OdePlugin private OdeScene m_scene; IntPtr ray; // the ray. we only need one for our lifetime + IntPtr Sphere; + IntPtr Box; + IntPtr Plane; - private const int ColisionContactGeomsPerTest = 5; + private int CollisionContactGeomsPerTest = 25; private const int DefaultMaxCount = 25; private const int MaxTimePerCallMS = 30; @@ -65,6 +68,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// ODE near callback delegate /// private d.NearCallback nearCallback; + private d.NearCallback nearProbeCallback; private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); private List m_contactResults = new List(); private RayFilterFlags CurrentRayFilter; @@ -74,169 +78,21 @@ namespace OpenSim.Region.Physics.OdePlugin { m_scene = pScene; nearCallback = near; + nearProbeCallback = nearProbe; ray = d.CreateRay(IntPtr.Zero, 1.0f); - d.GeomSetCategoryBits(ray,0); + d.GeomSetCategoryBits(ray, 0); + Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f); + d.GeomSetCategoryBits(Box, 0); + Sphere = d.CreateSphere(IntPtr.Zero,1.0f); + d.GeomSetCategoryBits(Sphere, 0); + Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f); + d.GeomSetCategoryBits(Sphere, 0); } - /// - /// Queues request for a raycast to all world - /// - /// Origin of Ray - /// Ray direction - /// Ray length - /// Return method to send the results - public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.Count = DefaultMaxCount; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - /// - /// Queues request for a raycast to particular part - /// - /// Origin of Ray - /// Ray direction - /// Ray length - /// Return method to send the results - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = DefaultMaxCount; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.Count = DefaultMaxCount; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = DefaultMaxCount; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - /// - /// Queues a raycast - /// - /// Origin of Ray - /// Ray normal - /// Ray length - /// - /// Return method to send the results - public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - - public void QueueRequest(Vector3 position, Vector3 direction, float length, int count,RayFilterFlags filter , RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = filter; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count,RayFilterFlags flags, RayCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = flags; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) - { - ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = RayFilterFlags.AllPrims; - - m_PendingRequests.Enqueue(req); - } - - public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) + public void QueueRequest(ODERayRequest req) { - ODERayRequest req = new ODERayRequest(); - req.geom = geom; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = count; - req.filter = RayFilterFlags.AllPrims; + if (req.Count == 0) + req.Count = DefaultMaxCount; m_PendingRequests.Enqueue(req); } @@ -272,21 +128,64 @@ namespace OpenSim.Region.Physics.OdePlugin CurrentRayFilter = req.filter; CurrentMaxCount = req.Count; + CollisionContactGeomsPerTest = req.Count & 0xffff; + closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); - d.GeomRaySetLength(ray, req.length); - d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); - d.GeomRaySetParams(ray, 0, backfacecull); - d.GeomRaySetClosestHit(ray, closestHit); + if (req.callbackMethod is ProbeBoxCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z); + d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z); + d.Quaternion qtmp; + qtmp.X = req.orientation.X; + qtmp.Y = req.orientation.Y; + qtmp.Z = req.orientation.Z; + qtmp.W = req.orientation.W; + d.GeomSetOffsetWorldQuaternion(Box, ref qtmp); + } + else if (req.callbackMethod is ProbeSphereCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + + d.GeomSphereSetRadius(Sphere, req.length); + d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z); + } + else if (req.callbackMethod is ProbePlaneCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + + d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length); + } + + else + { + if (CollisionContactGeomsPerTest > 25) + CollisionContactGeomsPerTest = 25; - if (req.callbackMethod is RaycastCallback) - // if we only want one get only one per colision pair saving memory - CurrentRayFilter |= RayFilterFlags.ClosestHit; + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + d.GeomRaySetParams(ray, 0, backfacecull); + d.GeomRaySetClosestHit(ray, closestHit); + + if (req.callbackMethod is RaycastCallback) + // if we only want one get only one per Collision pair saving memory + CurrentRayFilter |= RayFilterFlags.ClosestHit; + } + + if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) + unchecked + { + CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT; + } if (req.geom == IntPtr.Zero) { - // translate ray filter to colision flags + // translate ray filter to Collision flags catflags = 0; if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0) catflags |= CollisionCategories.VolumeDtc; @@ -303,15 +202,48 @@ namespace OpenSim.Region.Physics.OdePlugin if (catflags != 0) { - d.GeomSetCollideBits(ray, (uint)catflags); - doSpaceRay(req); + if (req.callbackMethod is ProbeBoxCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Box, (uint)catflags); + d.GeomSetCategoryBits(Box, (uint)catflags); + doProbe(req, Box); + } + else if (req.callbackMethod is ProbeSphereCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Sphere, (uint)catflags); + d.GeomSetCategoryBits(Sphere, (uint)catflags); + doProbe(req, Sphere); + } + else if (req.callbackMethod is ProbePlaneCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Plane, (uint)catflags); + d.GeomSetCategoryBits(Plane, (uint)catflags); + doPlane(req); + } + else + { + d.GeomSetCollideBits(ray, (uint)catflags); + doSpaceRay(req); + } } } else { // if we select a geom don't use filters - d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); - doGeomRay(req); + + if (req.callbackMethod is ProbePlaneCallback) + { + d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); + doPlane(req); + } + else + { + d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); + doGeomRay(req); + } } } @@ -396,6 +328,61 @@ namespace OpenSim.Region.Physics.OdePlugin } } + private void doProbe(ODERayRequest req, IntPtr probe) + { + // Collide tests + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + if (req.callbackMethod is ProbeBoxCallback) + ((ProbeBoxCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeSphereCallback) + ((ProbeSphereCallback)req.callbackMethod)(cresult); + } + + private void doPlane(ODERayRequest req) + { + // Collide tests + if (req.geom == IntPtr.Zero) + { + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + } + else + { + d.SpaceCollide2(Plane, req.geom, IntPtr.Zero, nearCallback); + } + + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + + ((ProbePlaneCallback)req.callbackMethod)(cresult); + } + /// /// Method that actually initiates the raycast with a geom /// @@ -450,7 +437,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) { IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; - if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest) + if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest) return false; IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); @@ -483,7 +470,7 @@ namespace OpenSim.Region.Physics.OdePlugin int count = 0; try { - count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); } catch (Exception e) { @@ -494,84 +481,210 @@ namespace OpenSim.Region.Physics.OdePlugin if (count == 0) return; + uint cat1 = d.GeomGetCategoryBits(g1); + uint cat2 = d.GeomGetCategoryBits(g2); + uint col1 = d.GeomGetCollideBits(g1); + uint col2 = d.GeomGetCollideBits(g2); + + uint ID = 0; PhysicsActor p2 = null; m_scene.actor_name_map.TryGetValue(g2, out p2); if (p2 == null) - { - /* - string name; - - if (!m_scene.geom_name_map.TryGetValue(g2, out name)) - return; - - if (name == "Terrain") - { - // land colision - if ((CurrentRayFilter & RayFilterFlags.land) == 0) - return; - } - else if (name == "Water") - { - if ((CurrentRayFilter & RayFilterFlags.water) == 0) - return; - } - else - return; - */ return; - } - else + + switch (p2.PhysicsActorType) { - switch (p2.PhysicsActorType) - { - case (int)ActorTypes.Prim: + case (int)ActorTypes.Prim: - RayFilterFlags thisFlags; + RayFilterFlags thisFlags; - if (p2.IsPhysical) - thisFlags = RayFilterFlags.physical; - else - thisFlags = RayFilterFlags.nonphysical; + if (p2.IsPhysical) + thisFlags = RayFilterFlags.physical; + else + thisFlags = RayFilterFlags.nonphysical; - if (p2.Phantom) - thisFlags |= RayFilterFlags.phantom; + if (p2.Phantom) + thisFlags |= RayFilterFlags.phantom; - if (p2.IsVolumeDtc) - thisFlags |= RayFilterFlags.volumedtc; + if (p2.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; - if ((thisFlags & CurrentRayFilter) == 0) - return; + if ((thisFlags & CurrentRayFilter) == 0) + return; - ID = ((OdePrim)p2).LocalID; - break; + ID = ((OdePrim)p2).LocalID; + break; - case (int)ActorTypes.Agent: + case (int)ActorTypes.Agent: - if ((CurrentRayFilter & RayFilterFlags.agent) == 0) - return; - else - ID = ((OdeCharacter)p2).LocalID; - break; + if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + return; + else + ID = ((OdeCharacter)p2).LocalID; + break; - case (int)ActorTypes.Ground: + case (int)ActorTypes.Ground: - if ((CurrentRayFilter & RayFilterFlags.land) == 0) - return; - break; + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + break; - case (int)ActorTypes.Water: + case (int)ActorTypes.Water: - if ((CurrentRayFilter & RayFilterFlags.water) == 0) - return; + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + break; + + default: + break; + } + + d.ContactGeom curcontact = new d.ContactGeom(); + + // closestHit for now only works for meshs, so must do it for others + if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) + { + // Loop all contacts, build results. + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) break; - default: + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Depth = curcontact.depth; + collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, + curcontact.normal.Z); + lock (m_contactResults) + { + m_contactResults.Add(collisionresult); + if (m_contactResults.Count >= CurrentMaxCount) + return; + } + } + } + else + { + // keep only closest contact + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Depth = float.MaxValue; + + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) break; + + if (curcontact.depth < collisionresult.Depth) + { + collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Depth = curcontact.depth; + collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, + curcontact.normal.Z); + } + } + + if (collisionresult.Depth != float.MaxValue) + { + lock (m_contactResults) + m_contactResults.Add(collisionresult); } } + } + + private void nearProbe(IntPtr space, IntPtr g1, IntPtr g2) + { + if (g1 == IntPtr.Zero || g1 == g2) + return; + + if (m_contactResults.Count >= CurrentMaxCount) + return; + + if (d.GeomIsSpace(g1)) + { + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearProbeCallback); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); + } + return; + } + + int count = 0; + try + { + count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); + return; + } + + if (count == 0) + return; + + uint ID = 0; + PhysicsActor p1 = null; + + m_scene.actor_name_map.TryGetValue(g1, out p1); + + if (p1 == null) + return; + + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Prim: + + RayFilterFlags thisFlags; + + if (p1.IsPhysical) + thisFlags = RayFilterFlags.physical; + else + thisFlags = RayFilterFlags.nonphysical; + + if (p1.Phantom) + thisFlags |= RayFilterFlags.phantom; + + if (p1.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; + + if ((thisFlags & CurrentRayFilter) == 0) + return; + + ID = ((OdePrim)p1).LocalID; + break; + + case (int)ActorTypes.Agent: + + if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + return; + else + ID = ((OdeCharacter)p1).LocalID; + break; + + case (int)ActorTypes.Ground: + + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + break; + + case (int)ActorTypes.Water: + + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + break; + + default: + break; + } d.ContactGeom curcontact = new d.ContactGeom(); @@ -638,6 +751,21 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomDestroy(ray); ray = IntPtr.Zero; } + if (Box != IntPtr.Zero) + { + d.GeomDestroy(Box); + Box = IntPtr.Zero; + } + if (Sphere != IntPtr.Zero) + { + d.GeomDestroy(Sphere); + Sphere = IntPtr.Zero; + } + if (Plane != IntPtr.Zero) + { + d.GeomDestroy(Plane); + Plane = IntPtr.Zero; + } } } @@ -650,5 +778,6 @@ namespace OpenSim.Region.Physics.OdePlugin public float length; public object callbackMethod; public RayFilterFlags filter; + public Quaternion orientation; } } \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs index fd3a3ba..9e23763 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -54,6 +54,20 @@ namespace OpenSim.Region.Physics.OdePlugin private static Vector3 SitAjust = new Vector3(0, 0, 0.4f); private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit; + private void RotAroundZ(float x, float y, ref Quaternion ori) + { + double ang = Math.Atan2(y, x); + ang *= 0.5d; + float s = (float)Math.Sin(ang); + float c = (float)Math.Cos(ang); + + ori.X = 0; + ori.Y = 0; + ori.Z = s; + ori.W = c; + } + + public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) { if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero) @@ -72,7 +86,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.AABB aabb; - Quaternion ori; + Quaternion ori = Quaternion.Identity; d.Quaternion qtmp; d.GeomCopyQuaternion(geom, out qtmp); Quaternion geomOri; @@ -86,9 +100,14 @@ namespace OpenSim.Region.Physics.OdePlugin geomInvOri.Z = -qtmp.Z; geomInvOri.W = qtmp.W; - Vector3 target = geopos + offset; - Vector3 rayDir = target - avCameraPosition; + Vector3 rayDir = geopos + offset - avCameraPosition; float raylen = rayDir.Length(); + if (raylen < 0.001f) + { + PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); + return; + } + float t = 1 / raylen; rayDir.X *= t; rayDir.Y *= t; @@ -98,9 +117,9 @@ namespace OpenSim.Region.Physics.OdePlugin List rayResults; rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); - if (rayResults.Count == 0 || rayResults[0].ConsumerID != actor.LocalID) + if (rayResults.Count == 0) { - d.GeomGetAABB(geom,out aabb); + d.GeomGetAABB(geom, out aabb); offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); ori = geomInvOri; offset *= geomInvOri; @@ -109,44 +128,45 @@ namespace OpenSim.Region.Physics.OdePlugin return; } - offset = rayResults[0].Pos - geopos; - double ang; - float s; - float c; d.GeomClassID geoclass = d.GeomGetClass(geom); if (geoclass == d.GeomClassID.SphereClass) { - float r = d.GeomSphereGetRadius(geom); + int status = 1; + float r = d.GeomSphereGetRadius(geom); offset.Normalize(); offset *= r; - ang = Math.Atan2(offset.Y, offset.X); - ang *= 0.5d; - s = (float)Math.Sin(ang); - c = (float)Math.Cos(ang); - - ori = new Quaternion(0, 0, s, c); + RotAroundZ(offset.X, offset.Y, ref ori); if (r < 0.4f) { offset = new Vector3(0, 0, r); } - else if (offset.Z < 0.4f) + else { - t = offset.Z; - float rsq = r * r; - - t = 1.0f / (rsq - t * t); - offset.X *= t; - offset.Y *= t; - offset.Z = 0.4f; - t = rsq - 0.16f; - offset.X *= t; - offset.Y *= t; + if (offset.Z < 0.4f) + { + t = offset.Z; + float rsq = r * r; + + t = 1.0f / (rsq - t * t); + offset.X *= t; + offset.Y *= t; + offset.Z = 0.4f; + t = rsq - 0.16f; + offset.X *= t; + offset.Y *= t; + } + else if (r > 0.8f && offset.Z > 0.8f * r) + { + status = 3; + avOffset.X = -avOffset.X; + avOffset.Z += 0.4f; + } } offset += avOffset * ori; @@ -154,27 +174,189 @@ namespace OpenSim.Region.Physics.OdePlugin ori = geomInvOri * ori; offset *= geomInvOri; - PhysicsSitResponse(1, actor.LocalID, offset, ori); + PhysicsSitResponse(status, actor.LocalID, offset, ori); return; } -/* - // contact normals aren't reliable on meshs or sculpts it seems - Vector3 norm = rayResults[0].Normal; + Vector3 norm = rayResults[0].Normal; - if (norm.Z < 0) + if (norm.Z < -0.4f) + { + PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); + return; + } + + float SitNormX = -rayDir.X; + float SitNormY = -rayDir.Y; + + Vector3 pivot = geopos + offset; + + float edgeNormalX = norm.X; + float edgeNormalY = norm.Y; + float edgeDirX = -rayDir.X; + float edgeDirY = -rayDir.Y; + Vector3 edgePos = rayResults[0].Pos; + float edgeDist = float.MaxValue; + + bool foundEdge = false; + + if (norm.Z < 0.5f) + { + float rayDist = 4.0f; + float curEdgeDist = 0.0f; + pivot = geopos + offset; + + for (int i = 0; i < 6; i++) { - PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); + pivot.X -= 0.005f * norm.X; + pivot.Y -= 0.005f * norm.Y; + pivot.Z -= 0.005f * norm.Z; + + rayDir.X = -norm.X * norm.Z; + rayDir.Y = -norm.Y * norm.Z; + rayDir.Z = 1.0f - norm.Z * norm.Z; + rayDir.Normalize(); + + rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); + if (rayResults.Count == 0) + break; + + curEdgeDist += rayResults[0].Depth; + + if (Math.Abs(rayResults[0].Normal.Z) < 0.7f) + { + rayDist -= rayResults[0].Depth; + if (rayDist < 0f) + break; + + pivot = rayResults[0].Pos; + norm = rayResults[0].Normal; + edgeNormalX = norm.X; + edgeNormalY = norm.Y; + edgeDirX = rayDir.X; + edgeDirY = rayDir.Y; + } + else + { + foundEdge = true; + if (curEdgeDist < edgeDist) + { + edgeDist = curEdgeDist; + edgePos = rayResults[0].Pos; + } + break; + } + } + + if (!foundEdge) + { + PhysicsSitResponse(0, actor.LocalID, offset, ori); return; } -*/ + avOffset.X *= 0.5f; + } - ang = Math.Atan2(-rayDir.Y, -rayDir.X); - ang *= 0.5d; - s = (float)Math.Sin(ang); - c = (float)Math.Cos(ang); + else if (norm.Z > 0.866f) + { + float toCamBaseX = avCameraPosition.X - pivot.X; + float toCamBaseY = avCameraPosition.Y - pivot.Y; + float toCamX = toCamBaseX; + float toCamY = toCamBaseY; + + for (int j = 0; j < 4; j++) + { + float rayDist = 1.0f; + float curEdgeDist = 0.0f; + pivot = geopos + offset; + + for (int i = 0; i < 3; i++) + { + pivot.Z -= 0.005f; + rayDir.X = toCamX; + rayDir.Y = toCamY; + rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z; + rayDir.Normalize(); + + rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); + if (rayResults.Count == 0) + break; + + curEdgeDist += rayResults[0].Depth; + + if (rayResults[0].Normal.Z > 0.5f) + { + rayDist -= rayResults[0].Depth; + if (rayDist < 0f) + break; + + pivot = rayResults[0].Pos; + norm = rayResults[0].Normal; + } + else + { + foundEdge = true; + if (curEdgeDist < edgeDist) + { + edgeDist = curEdgeDist; + edgeNormalX = rayResults[0].Normal.X; + edgeNormalY = rayResults[0].Normal.Y; + edgeDirX = rayDir.X; + edgeDirY = rayDir.Y; + edgePos = rayResults[0].Pos; + } + break; + } + } + if (foundEdge && edgeDist < 0.2f) + break; + + switch (j) + { + case 0: + toCamX = -toCamBaseY; + toCamY = toCamBaseX; + break; + case 1: + toCamX = toCamBaseY; + toCamY = -toCamBaseX; + break; + case 2: + toCamX = -toCamBaseX; + toCamY = -toCamBaseY; + break; + default: + break; + } + } + + if (!foundEdge) + { + avOffset.X = -avOffset.X; + avOffset.Z += 0.4f; + + RotAroundZ(SitNormX, SitNormY, ref ori); + + offset += avOffset * ori; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(3, actor.LocalID, offset, ori); + return; + } + avOffset.X *= 0.5f; + } + + SitNormX = edgeNormalX; + SitNormY = edgeNormalY; + offset = edgePos - geopos; + if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0) + { + SitNormX = -SitNormX; + SitNormY = -SitNormY; + } - ori = new Quaternion(0, 0, s, c); + RotAroundZ(SitNormX, SitNormY, ref ori); offset += avOffset * ori; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index d344d4d..d045b59 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2579,7 +2579,16 @@ namespace OpenSim.Region.Physics.OdePlugin { if (retMethod != null) { - m_rayCastManager.QueueRequest(position, direction, length, retMethod); + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + req.filter = RayFilterFlags.All; + + m_rayCastManager.QueueRequest(req); } } @@ -2587,29 +2596,51 @@ namespace OpenSim.Region.Physics.OdePlugin { if (retMethod != null) { - m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = RayFilterFlags.All; + + m_rayCastManager.QueueRequest(req); } } - // don't like this + public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) { - ContactResult[] ourResults = null; + List ourresults = new List(); + object SyncObject = new object(); + RayCallback retMethod = delegate(List results) { - ourResults = new ContactResult[results.Count]; - results.CopyTo(ourResults, 0); + lock (SyncObject) + { + ourresults = results; + Monitor.PulseAll(SyncObject); + } }; - int waitTime = 0; - m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); - while (ourResults == null && waitTime < 1000) + + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = RayFilterFlags.All; + + lock (SyncObject) { - Thread.Sleep(1); - waitTime++; + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return null; + else + return ourresults; } - if (ourResults == null) - return new List(); - return new List(ourResults); } public override bool SuportsRaycastWorldFiltered() @@ -2631,9 +2662,18 @@ namespace OpenSim.Region.Physics.OdePlugin } }; + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = filter; + lock (SyncObject) { - m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod); + m_rayCastManager.QueueRequest(req); if (!Monitor.Wait(SyncObject, 500)) return null; else @@ -2641,73 +2681,163 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) { - if (retMethod != null && actor !=null) + if (actor == null) + return new List(); + + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return new List(); + + if (geom == IntPtr.Zero) + return new List(); + + List ourResults = null; + object SyncObject = new object(); + + RayCallback retMethod = delegate(List results) { - IntPtr geom; - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - else - return; - if (geom == IntPtr.Zero) - return; - m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod); + lock (SyncObject) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); } + + if (ourResults == null) + return new List(); + return ourResults; } - public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + public override List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) { - if (retMethod != null && actor != null) + List ourResults = null; + object SyncObject = new object(); + + ProbeBoxCallback retMethod = delegate(List results) { - IntPtr geom; - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - else - return; - if (geom == IntPtr.Zero) - return; + lock (SyncObject) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.Normal = size; + req.Origin = position; + req.orientation = orientation; + req.Count = Count; + req.filter = flags; - m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); } + + if (ourResults == null) + return new List(); + return ourResults; } - - public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) + + public override List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) + { + List ourResults = null; + object SyncObject = new object(); + + ProbeSphereCallback retMethod = delegate(List results) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + }; + + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = radius; + req.Origin = position; + req.Count = Count; + req.filter = flags; + + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) { + IntPtr geom = IntPtr.Zero;; + if (actor != null) { - IntPtr geom; if (actor is OdePrim) geom = ((OdePrim)actor).prim_geom; else if (actor is OdeCharacter) geom = ((OdePrim)actor).prim_geom; - else - return new List(); - if (geom == IntPtr.Zero) - return new List(); + } - ContactResult[] ourResults = null; - RayCallback retMethod = delegate(List results) - { - ourResults = new ContactResult[results.Count]; - results.CopyTo(ourResults, 0); - }; - int waitTime = 0; - m_rayCastManager.QueueRequest(geom,position, direction, length, Count, flags, retMethod); - while (ourResults == null && waitTime < 1000) - { - Thread.Sleep(1); - waitTime++; - } - if (ourResults == null) + List ourResults = null; + object SyncObject = new object(); + + ProbePlaneCallback retMethod = delegate(List results) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + }; + + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = plane.W; + req.Normal.X = plane.X; + req.Normal.Y = plane.Y; + req.Normal.Z = plane.Z; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) return new List(); - return new List(ourResults); } - return new List(); + + if (ourResults == null) + return new List(); + return ourResults; } public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) -- cgit v1.1 From be6b6bf191718f7c674297a804fa1a6cb4196a5a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 2 Jan 2013 20:06:57 +0000 Subject: add a lock to CollisionEventsThisFrame --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 49 +++++++++++++--------- 1 file changed, 29 insertions(+), 20 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index e1d694e..f7e4c1c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1540,8 +1540,11 @@ namespace OpenSim.Region.Physics.OdePlugin { if (CollisionEventsThisFrame != null) { - CollisionEventsThisFrame.Clear(); - CollisionEventsThisFrame = null; + lock (CollisionEventsThisFrame) + { + CollisionEventsThisFrame.Clear(); + CollisionEventsThisFrame = null; + } } m_eventsubscription = 0; } @@ -1550,8 +1553,11 @@ namespace OpenSim.Region.Physics.OdePlugin { if (CollisionEventsThisFrame == null) CollisionEventsThisFrame = new CollisionEventUpdate(); - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - _parent_scene.AddCollisionEventReporting(this); + lock (CollisionEventsThisFrame) + { + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + _parent_scene.AddCollisionEventReporting(this); + } } public void SendCollisions() @@ -1559,26 +1565,29 @@ namespace OpenSim.Region.Physics.OdePlugin if (CollisionEventsThisFrame == null) return; - if (m_cureventsubscription < m_eventsubscription) - return; + lock (CollisionEventsThisFrame) + { + if (m_cureventsubscription < m_eventsubscription) + return; - m_cureventsubscription = 0; + m_cureventsubscription = 0; - int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; - if (!SentEmptyCollisionsEvent || ncolisions > 0) - { - base.SendCollisionUpdate(CollisionEventsThisFrame); - - if (ncolisions == 0) + if (!SentEmptyCollisionsEvent || ncolisions > 0) { - SentEmptyCollisionsEvent = true; - _parent_scene.RemoveCollisionEventReporting(this); - } - else - { - SentEmptyCollisionsEvent = false; - CollisionEventsThisFrame.Clear(); + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (ncolisions == 0) + { + SentEmptyCollisionsEvent = true; + _parent_scene.RemoveCollisionEventReporting(this); + } + else + { + SentEmptyCollisionsEvent = false; + CollisionEventsThisFrame.Clear(); + } } } } -- cgit v1.1 From a504704071b5302601240854c32fcc3d3c5cd3b8 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 2 Jan 2013 21:59:19 +0000 Subject: i try to fix avatar orientation in some cases.. --- OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs index 9e23763..d6f6681 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -233,8 +233,8 @@ namespace OpenSim.Region.Physics.OdePlugin norm = rayResults[0].Normal; edgeNormalX = norm.X; edgeNormalY = norm.Y; - edgeDirX = rayDir.X; - edgeDirY = rayDir.Y; + edgeDirX = -rayDir.X; + edgeDirY = -rayDir.Y; } else { -- cgit v1.1 From c961cfe19e6dfaf26a0117748edbd66fc2d92823 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 3 Jan 2013 21:53:21 +0000 Subject: bug fix, cleanup... --- .../UbitOdePlugin/ODERayCastRequestManager.cs | 6 +-- .../Region/Physics/UbitOdePlugin/ODESitAvatar.cs | 51 ++++++++-------------- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 15 ++++++- 3 files changed, 34 insertions(+), 38 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 31757a9..6e9281b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -144,7 +144,7 @@ namespace OpenSim.Region.Physics.OdePlugin qtmp.Y = req.orientation.Y; qtmp.Z = req.orientation.Z; qtmp.W = req.orientation.W; - d.GeomSetOffsetWorldQuaternion(Box, ref qtmp); + d.GeomSetQuaternion(Box, ref qtmp); } else if (req.callbackMethod is ProbeSphereCallback) { @@ -480,12 +480,12 @@ namespace OpenSim.Region.Physics.OdePlugin if (count == 0) return; - +/* uint cat1 = d.GeomGetCategoryBits(g1); uint cat2 = d.GeomGetCategoryBits(g2); uint col1 = d.GeomGetCollideBits(g1); uint col2 = d.GeomGetCollideBits(g2); - +*/ uint ID = 0; PhysicsActor p2 = null; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs index d6f6681..ecc732a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -78,36 +78,20 @@ namespace OpenSim.Region.Physics.OdePlugin IntPtr geom = ((OdePrim)actor).prim_geom; - d.Vector3 dtmp = d.GeomGetPosition(geom); - Vector3 geopos; - geopos.X = dtmp.X; - geopos.Y = dtmp.Y; - geopos.Z = dtmp.Z; + Vector3 geopos = d.GeomGetPositionOMV(geom); + Quaternion geomOri = d.GeomGetQuaternionOMV(geom); + Quaternion geomInvOri = Quaternion.Conjugate(geomOri); - - d.AABB aabb; Quaternion ori = Quaternion.Identity; - d.Quaternion qtmp; - d.GeomCopyQuaternion(geom, out qtmp); - Quaternion geomOri; - geomOri.X = qtmp.X; - geomOri.Y = qtmp.Y; - geomOri.Z = qtmp.Z; - geomOri.W = qtmp.W; - Quaternion geomInvOri; - geomInvOri.X = -qtmp.X; - geomInvOri.Y = -qtmp.Y; - geomInvOri.Z = -qtmp.Z; - geomInvOri.W = qtmp.W; Vector3 rayDir = geopos + offset - avCameraPosition; + float raylen = rayDir.Length(); if (raylen < 0.001f) { PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); return; } - float t = 1 / raylen; rayDir.X *= t; rayDir.Y *= t; @@ -119,22 +103,25 @@ namespace OpenSim.Region.Physics.OdePlugin rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); if (rayResults.Count == 0) { +/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim + d.AABB aabb; d.GeomGetAABB(geom, out aabb); offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); ori = geomInvOri; offset *= geomInvOri; - PhysicsSitResponse(1, actor.LocalID, offset, ori); +*/ + PhysicsSitResponse(0, actor.LocalID, offset, ori); return; } + int status = 1; offset = rayResults[0].Pos - geopos; d.GeomClassID geoclass = d.GeomGetClass(geom); if (geoclass == d.GeomClassID.SphereClass) { - int status = 1; float r = d.GeomSphereGetRadius(geom); offset.Normalize(); @@ -165,7 +152,7 @@ namespace OpenSim.Region.Physics.OdePlugin { status = 3; avOffset.X = -avOffset.X; - avOffset.Z += 0.4f; + avOffset.Z *= 1.6f; } } @@ -186,6 +173,7 @@ namespace OpenSim.Region.Physics.OdePlugin return; } + float SitNormX = -rayDir.X; float SitNormY = -rayDir.Y; @@ -204,7 +192,6 @@ namespace OpenSim.Region.Physics.OdePlugin { float rayDist = 4.0f; float curEdgeDist = 0.0f; - pivot = geopos + offset; for (int i = 0; i < 6; i++) { @@ -239,11 +226,8 @@ namespace OpenSim.Region.Physics.OdePlugin else { foundEdge = true; - if (curEdgeDist < edgeDist) - { - edgeDist = curEdgeDist; - edgePos = rayResults[0].Pos; - } + edgeDist = curEdgeDist; + edgePos = rayResults[0].Pos; break; } } @@ -267,7 +251,6 @@ namespace OpenSim.Region.Physics.OdePlugin { float rayDist = 1.0f; float curEdgeDist = 0.0f; - pivot = geopos + offset; for (int i = 0; i < 3; i++) { @@ -310,6 +293,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (foundEdge && edgeDist < 0.2f) break; + pivot = geopos + offset; + switch (j) { case 0: @@ -332,7 +317,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!foundEdge) { avOffset.X = -avOffset.X; - avOffset.Z += 0.4f; + avOffset.Z *= 1.6f; RotAroundZ(SitNormX, SitNormY, ref ori); @@ -349,7 +334,6 @@ namespace OpenSim.Region.Physics.OdePlugin SitNormX = edgeNormalX; SitNormY = edgeNormalY; - offset = edgePos - geopos; if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0) { SitNormX = -SitNormX; @@ -358,7 +342,8 @@ namespace OpenSim.Region.Physics.OdePlugin RotAroundZ(SitNormX, SitNormY, ref ori); - offset += avOffset * ori; + offset = edgePos + avOffset * ori; + offset -= geopos; ori = geomInvOri * ori; offset *= geomInvOri; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 403a4ce..10d7d50 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using System; using System.Runtime.InteropServices; using System.Security; - +using OMV = OpenMetaverse; namespace OdeAPI { //#if dDOUBLE @@ -925,9 +925,20 @@ namespace OdeAPI { unsafe { return *(GeomGetPositionUnsafe(geom)); } } + public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom) + { + Vector3 vtmp = GeomGetPosition(geom); + return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom) + { + Quaternion qtmp; + GeomCopyQuaternion(geom, out qtmp); + return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X); -- cgit v1.1 From dc6b7bb5c9d4ba26a043566f3c6f969d56268132 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 6 Jan 2013 12:07:16 +0000 Subject: don't try to read geom positions outside main ode thread :( --- .../Region/Physics/UbitOdePlugin/ODESitAvatar.cs | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs index ecc732a..e9023c3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -78,8 +78,12 @@ namespace OpenSim.Region.Physics.OdePlugin IntPtr geom = ((OdePrim)actor).prim_geom; - Vector3 geopos = d.GeomGetPositionOMV(geom); - Quaternion geomOri = d.GeomGetQuaternionOMV(geom); +// Vector3 geopos = d.GeomGetPositionOMV(geom); +// Quaternion geomOri = d.GeomGetQuaternionOMV(geom); + + Vector3 geopos = actor.Position; + Quaternion geomOri = actor.Orientation; + Quaternion geomInvOri = Quaternion.Conjugate(geomOri); Quaternion ori = Quaternion.Identity; @@ -116,6 +120,7 @@ namespace OpenSim.Region.Physics.OdePlugin } int status = 1; + offset = rayResults[0].Pos - geopos; d.GeomClassID geoclass = d.GeomGetClass(geom); @@ -191,13 +196,12 @@ namespace OpenSim.Region.Physics.OdePlugin if (norm.Z < 0.5f) { float rayDist = 4.0f; - float curEdgeDist = 0.0f; for (int i = 0; i < 6; i++) { - pivot.X -= 0.005f * norm.X; - pivot.Y -= 0.005f * norm.Y; - pivot.Z -= 0.005f * norm.Z; + pivot.X -= 0.01f * norm.X; + pivot.Y -= 0.01f * norm.Y; + pivot.Z -= 0.01f * norm.Z; rayDir.X = -norm.X * norm.Z; rayDir.Y = -norm.Y * norm.Z; @@ -208,8 +212,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (rayResults.Count == 0) break; - curEdgeDist += rayResults[0].Depth; - if (Math.Abs(rayResults[0].Normal.Z) < 0.7f) { rayDist -= rayResults[0].Depth; @@ -226,7 +228,6 @@ namespace OpenSim.Region.Physics.OdePlugin else { foundEdge = true; - edgeDist = curEdgeDist; edgePos = rayResults[0].Pos; break; } @@ -254,7 +255,7 @@ namespace OpenSim.Region.Physics.OdePlugin for (int i = 0; i < 3; i++) { - pivot.Z -= 0.005f; + pivot.Z -= 0.01f; rayDir.X = toCamX; rayDir.Y = toCamY; rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z; -- cgit v1.1 From 982328b4ed2f632765e3c3d3bd1cc1110fdb91fa Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 7 Jan 2013 10:56:30 +0000 Subject: exclude avatars from unfiltered RaycastWorld --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index d045b59..f58870a 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2586,7 +2586,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = 0; - req.filter = RayFilterFlags.All; + req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; m_rayCastManager.QueueRequest(req); } @@ -2603,7 +2603,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = Count; - req.filter = RayFilterFlags.All; + req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; m_rayCastManager.QueueRequest(req); } @@ -2631,7 +2631,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = Count; - req.filter = RayFilterFlags.All; + req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; lock (SyncObject) { -- cgit v1.1 From d2cb2da7909aa79260af52a7abbc7b491c5495c2 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 7 Jan 2013 11:03:36 +0000 Subject: also exclude land --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index f58870a..0d18adb 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2586,7 +2586,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = 0; - req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; + req.filter = RayFilterFlags.AllPrims; m_rayCastManager.QueueRequest(req); } @@ -2603,7 +2603,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = Count; - req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; + req.filter = RayFilterFlags.AllPrims; m_rayCastManager.QueueRequest(req); } @@ -2631,7 +2631,7 @@ namespace OpenSim.Region.Physics.OdePlugin req.Normal = direction; req.Origin = position; req.Count = Count; - req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land; + req.filter = RayFilterFlags.AllPrims; lock (SyncObject) { -- cgit v1.1 From d5066ae6787ac860e673a91bf207bf2b397a2714 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 8 Jan 2013 00:21:09 +0000 Subject: * DAnger, TEST * change camera plane collision detection. Still bounces on * prim edges due to camera lag --- .../UbitOdePlugin/ODERayCastRequestManager.cs | 41 +++++++++++++++------- 1 file changed, 29 insertions(+), 12 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 6e9281b..7fe3109 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -173,8 +173,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomRaySetClosestHit(ray, closestHit); if (req.callbackMethod is RaycastCallback) + { // if we only want one get only one per Collision pair saving memory CurrentRayFilter |= RayFilterFlags.ClosestHit; + d.GeomRaySetClosestHit(ray, 1); + } + else + d.GeomRaySetClosestHit(ray, closestHit); } if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) @@ -555,10 +560,13 @@ namespace OpenSim.Region.Physics.OdePlugin ContactResult collisionresult = new ContactResult(); collisionresult.ConsumerID = ID; - collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Pos.X = curcontact.pos.X; + collisionresult.Pos.Y = curcontact.pos.Y; + collisionresult.Pos.Z = curcontact.pos.Z; collisionresult.Depth = curcontact.depth; - collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, - curcontact.normal.Z); + collisionresult.Normal.X = curcontact.normal.X; + collisionresult.Normal.Y = curcontact.normal.Y; + collisionresult.Normal.Z = curcontact.normal.Z; lock (m_contactResults) { m_contactResults.Add(collisionresult); @@ -581,10 +589,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (curcontact.depth < collisionresult.Depth) { - collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Pos.X = curcontact.pos.X; + collisionresult.Pos.Y = curcontact.pos.Y; + collisionresult.Pos.Z = curcontact.pos.Z; collisionresult.Depth = curcontact.depth; - collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, - curcontact.normal.Z); + collisionresult.Normal.X = curcontact.normal.X; + collisionresult.Normal.Y = curcontact.normal.Y; + collisionresult.Normal.Z = curcontact.normal.Z; } } @@ -699,10 +710,13 @@ namespace OpenSim.Region.Physics.OdePlugin ContactResult collisionresult = new ContactResult(); collisionresult.ConsumerID = ID; - collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Pos.X = curcontact.pos.X; + collisionresult.Pos.Y = curcontact.pos.Y; + collisionresult.Pos.Z = curcontact.pos.Z; collisionresult.Depth = curcontact.depth; - collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, - curcontact.normal.Z); + collisionresult.Normal.X = curcontact.normal.X; + collisionresult.Normal.Y = curcontact.normal.Y; + collisionresult.Normal.Z = curcontact.normal.Z; lock (m_contactResults) { m_contactResults.Add(collisionresult); @@ -725,10 +739,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (curcontact.depth < collisionresult.Depth) { - collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Pos.X = curcontact.pos.X; + collisionresult.Pos.Y = curcontact.pos.Y; + collisionresult.Pos.Z = curcontact.pos.Z; collisionresult.Depth = curcontact.depth; - collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, - curcontact.normal.Z); + collisionresult.Normal.X = curcontact.normal.X; + collisionresult.Normal.Y = curcontact.normal.Y; + collisionresult.Normal.Z = curcontact.normal.Z; } } -- cgit v1.1 From 2028787c0dee934eb97c07fe7d16909dc92789c3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 13 Jan 2013 12:25:41 +0000 Subject: prevent potencial invalid refs --- .../UbitOdePlugin/ODERayCastRequestManager.cs | 207 +++++---------------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 16 +- 2 files changed, 53 insertions(+), 170 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 7fe3109..4f598ea 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -68,7 +68,6 @@ namespace OpenSim.Region.Physics.OdePlugin /// ODE near callback delegate /// private d.NearCallback nearCallback; - private d.NearCallback nearProbeCallback; private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); private List m_contactResults = new List(); private RayFilterFlags CurrentRayFilter; @@ -78,7 +77,6 @@ namespace OpenSim.Region.Physics.OdePlugin { m_scene = pScene; nearCallback = near; - nearProbeCallback = nearProbe; ray = d.CreateRay(IntPtr.Zero, 1.0f); d.GeomSetCategoryBits(ray, 0); Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f); @@ -125,6 +123,24 @@ namespace OpenSim.Region.Physics.OdePlugin { if (req.callbackMethod != null) { + IntPtr geom = IntPtr.Zero; + if (req.actor != null) + { + if (m_scene.haveActor(req.actor)) + { + if (req.actor is OdePrim) + geom = ((OdePrim)req.actor).prim_geom; + else if (req.actor is OdeCharacter) + geom = ((OdePrim)req.actor).prim_geom; + } + if (geom == IntPtr.Zero) + { + NoContacts(req); + continue; + } + } + + CurrentRayFilter = req.filter; CurrentMaxCount = req.Count; @@ -188,7 +204,7 @@ namespace OpenSim.Region.Physics.OdePlugin CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT; } - if (req.geom == IntPtr.Zero) + if (geom == IntPtr.Zero) { // translate ray filter to Collision flags catflags = 0; @@ -226,7 +242,7 @@ namespace OpenSim.Region.Physics.OdePlugin catflags |= CollisionCategories.Space; d.GeomSetCollideBits(Plane, (uint)catflags); d.GeomSetCategoryBits(Plane, (uint)catflags); - doPlane(req); + doPlane(req,IntPtr.Zero); } else { @@ -242,12 +258,12 @@ namespace OpenSim.Region.Physics.OdePlugin if (req.callbackMethod is ProbePlaneCallback) { d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); - doPlane(req); + doPlane(req,geom); } else { d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); - doGeomRay(req); + doGeomRay(req,geom); } } } @@ -267,6 +283,23 @@ namespace OpenSim.Region.Physics.OdePlugin /// /// + private void NoContacts(ODERayRequest req) + { + if (req.callbackMethod is RaycastCallback) + { + ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero); + return; + } + List cresult = new List(); + + if (req.callbackMethod is RayCallback) + ((RayCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeBoxCallback) + ((ProbeBoxCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeSphereCallback) + ((ProbeSphereCallback)req.callbackMethod)(cresult); + } + private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton; // private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; @@ -358,10 +391,10 @@ namespace OpenSim.Region.Physics.OdePlugin ((ProbeSphereCallback)req.callbackMethod)(cresult); } - private void doPlane(ODERayRequest req) + private void doPlane(ODERayRequest req,IntPtr geom) { // Collide tests - if (req.geom == IntPtr.Zero) + if (geom == IntPtr.Zero) { if ((CurrentRayFilter & FilterActiveSpace) != 0) { @@ -375,7 +408,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - d.SpaceCollide2(Plane, req.geom, IntPtr.Zero, nearCallback); + d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback); } List cresult = new List(m_contactResults.Count); @@ -392,10 +425,10 @@ namespace OpenSim.Region.Physics.OdePlugin /// Method that actually initiates the raycast with a geom /// /// - private void doGeomRay(ODERayRequest req) + private void doGeomRay(ODERayRequest req, IntPtr geom) { // Collide test - d.SpaceCollide2(ray, req.geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test + d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test if (req.callbackMethod is RaycastCallback) { @@ -607,156 +640,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - private void nearProbe(IntPtr space, IntPtr g1, IntPtr g2) - { - if (g1 == IntPtr.Zero || g1 == g2) - return; - - if (m_contactResults.Count >= CurrentMaxCount) - return; - - if (d.GeomIsSpace(g1)) - { - try - { - d.SpaceCollide2(g1, g2, IntPtr.Zero, nearProbeCallback); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); - } - return; - } - - int count = 0; - try - { - count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); - return; - } - - if (count == 0) - return; - - uint ID = 0; - PhysicsActor p1 = null; - - m_scene.actor_name_map.TryGetValue(g1, out p1); - - if (p1 == null) - return; - - switch (p1.PhysicsActorType) - { - case (int)ActorTypes.Prim: - - RayFilterFlags thisFlags; - - if (p1.IsPhysical) - thisFlags = RayFilterFlags.physical; - else - thisFlags = RayFilterFlags.nonphysical; - - if (p1.Phantom) - thisFlags |= RayFilterFlags.phantom; - - if (p1.IsVolumeDtc) - thisFlags |= RayFilterFlags.volumedtc; - - if ((thisFlags & CurrentRayFilter) == 0) - return; - - ID = ((OdePrim)p1).LocalID; - break; - - case (int)ActorTypes.Agent: - - if ((CurrentRayFilter & RayFilterFlags.agent) == 0) - return; - else - ID = ((OdeCharacter)p1).LocalID; - break; - - case (int)ActorTypes.Ground: - - if ((CurrentRayFilter & RayFilterFlags.land) == 0) - return; - break; - - case (int)ActorTypes.Water: - - if ((CurrentRayFilter & RayFilterFlags.water) == 0) - return; - break; - - default: - break; - } - - d.ContactGeom curcontact = new d.ContactGeom(); - - // closestHit for now only works for meshs, so must do it for others - if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) - { - // Loop all contacts, build results. - for (int i = 0; i < count; i++) - { - if (!GetCurContactGeom(i, ref curcontact)) - break; - - ContactResult collisionresult = new ContactResult(); - collisionresult.ConsumerID = ID; - collisionresult.Pos.X = curcontact.pos.X; - collisionresult.Pos.Y = curcontact.pos.Y; - collisionresult.Pos.Z = curcontact.pos.Z; - collisionresult.Depth = curcontact.depth; - collisionresult.Normal.X = curcontact.normal.X; - collisionresult.Normal.Y = curcontact.normal.Y; - collisionresult.Normal.Z = curcontact.normal.Z; - lock (m_contactResults) - { - m_contactResults.Add(collisionresult); - if (m_contactResults.Count >= CurrentMaxCount) - return; - } - } - } - else - { - // keep only closest contact - ContactResult collisionresult = new ContactResult(); - collisionresult.ConsumerID = ID; - collisionresult.Depth = float.MaxValue; - - for (int i = 0; i < count; i++) - { - if (!GetCurContactGeom(i, ref curcontact)) - break; - - if (curcontact.depth < collisionresult.Depth) - { - collisionresult.Pos.X = curcontact.pos.X; - collisionresult.Pos.Y = curcontact.pos.Y; - collisionresult.Pos.Z = curcontact.pos.Z; - collisionresult.Depth = curcontact.depth; - collisionresult.Normal.X = curcontact.normal.X; - collisionresult.Normal.Y = curcontact.normal.Y; - collisionresult.Normal.Z = curcontact.normal.Z; - } - } - - if (collisionresult.Depth != float.MaxValue) - { - lock (m_contactResults) - m_contactResults.Add(collisionresult); - } - } - } - /// /// Dereference the creator scene so that it can be garbage collected if needed. /// @@ -788,7 +671,7 @@ namespace OpenSim.Region.Physics.OdePlugin public struct ODERayRequest { - public IntPtr geom; + public PhysicsActor actor; public Vector3 Origin; public Vector3 Normal; public int Count; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 0d18adb..5113210 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2580,7 +2580,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (retMethod != null) { ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; + req.actor = null; req.callbackMethod = retMethod; req.length = length; req.Normal = direction; @@ -2597,7 +2597,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (retMethod != null) { ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; + req.actor = null; req.callbackMethod = retMethod; req.length = length; req.Normal = direction; @@ -2625,7 +2625,7 @@ namespace OpenSim.Region.Physics.OdePlugin }; ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; + req.actor = null; req.callbackMethod = retMethod; req.length = length; req.Normal = direction; @@ -2663,7 +2663,7 @@ namespace OpenSim.Region.Physics.OdePlugin }; ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; + req.actor = null; req.callbackMethod = retMethod; req.length = length; req.Normal = direction; @@ -2710,7 +2710,7 @@ namespace OpenSim.Region.Physics.OdePlugin }; ODERayRequest req = new ODERayRequest(); - req.geom = geom; + req.actor = actor; req.callbackMethod = retMethod; req.length = length; req.Normal = direction; @@ -2745,7 +2745,7 @@ namespace OpenSim.Region.Physics.OdePlugin }; ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; + req.actor = null; req.callbackMethod = retMethod; req.Normal = size; req.Origin = position; @@ -2777,7 +2777,7 @@ namespace OpenSim.Region.Physics.OdePlugin }; ODERayRequest req = new ODERayRequest(); - req.geom = IntPtr.Zero; + req.actor = null; req.callbackMethod = retMethod; req.length = radius; req.Origin = position; @@ -2819,7 +2819,7 @@ namespace OpenSim.Region.Physics.OdePlugin }; ODERayRequest req = new ODERayRequest(); - req.geom = geom; + req.actor = null; req.callbackMethod = retMethod; req.length = plane.W; req.Normal.X = plane.X; -- cgit v1.1 From c75508ec8d236b45c65c80d479ed7c24dd3343ce Mon Sep 17 00:00:00 2001 From: Melanie Date: Wed, 23 Jan 2013 20:29:05 +0100 Subject: Fix a type (Suports => Supports). Also put the normal terrain collision check into the physics check patch for now since physics doesn't properly return land for some reason (as tested by Nebadon) --- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index d24ab2a..57e2d20 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -350,7 +350,7 @@ namespace OpenSim.Region.Physics.Manager return null; } - public virtual bool SuportsRaycastWorldFiltered() + public virtual bool SupportsRaycastWorldFiltered() { return false; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 5113210..510cbe9 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2643,7 +2643,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override bool SuportsRaycastWorldFiltered() + public override bool SupportsRaycastWorldFiltered() { return true; } -- cgit v1.1 From bfac09849ff4b14f5df4bab4fffdbaea0d3b6a9e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 26 Jan 2013 06:12:26 +0000 Subject: bug fix: crash when too many collisions on a ode step --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 76 +++++++++++++----------- 1 file changed, 40 insertions(+), 36 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 510cbe9..754bc86 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -995,63 +995,67 @@ namespace OpenSim.Region.Physics.OdePlugin d.ContactGeom maxContact = curContact; -// if (IgnoreNegSides && curContact.side1 < 0) -// maxContact.depth = float.MinValue; + // if (IgnoreNegSides && curContact.side1 < 0) + // maxContact.depth = float.MinValue; d.ContactGeom minContact = curContact; -// if (IgnoreNegSides && curContact.side1 < 0) -// minContact.depth = float.MaxValue; + // if (IgnoreNegSides && curContact.side1 < 0) + // minContact.depth = float.MaxValue; IntPtr Joint; bool FeetCollision = false; int ncontacts = 0; - int i = 0; + int i = 0; - while (true) - { - if (m_global_contactcount >= maxContactsbeforedeath) - break; + while (true) + { // if (!(IgnoreNegSides && curContact.side1 < 0)) + { + bool noskip = true; + if (dop1ava) { - bool noskip = true; - if (dop1ava) - { - if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact, ref FeetCollision))) + if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision))) - noskip = false; - } - else if (dop2ava) - { - if (!(((OdeCharacter)p2).Collide(g2,true, ref curContact, ref FeetCollision))) - noskip = false; - } + noskip = false; + } + else if (dop2ava) + { + if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision))) + noskip = false; + } - if (noskip) - { - m_global_contactcount++; - ncontacts++; + if (noskip) + { + m_global_contactcount++; + if (m_global_contactcount >= maxContactsbeforedeath) + break; - Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); - d.JointAttach(Joint, b1, b2); + ncontacts++; - if (curContact.depth > maxContact.depth) - maxContact = curContact; + Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); + if (Joint == IntPtr.Zero) + break; - if (curContact.depth < minContact.depth) - minContact = curContact; - } - } + d.JointAttach(Joint, b1, b2); - if (++i >= count) - break; + if (curContact.depth > maxContact.depth) + maxContact = curContact; - if (!GetCurContactGeom(i, ref curContact)) - break; + if (curContact.depth < minContact.depth) + minContact = curContact; + } } + if (++i >= count) + break; + + if (!GetCurContactGeom(i, ref curContact)) + break; + } + if (ncontacts > 0) { ContactPoint maxDepthContact = new ContactPoint( -- cgit v1.1 From 765b0e4382b1e7e413d33a8f0f7669259f7bf170 Mon Sep 17 00:00:00 2001 From: Melanie Date: Thu, 7 Feb 2013 22:20:17 +0100 Subject: Rename "Bounce" to "Restitution" in PhysicsActor as well. It appears these values are not even used. --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 9338130..dcb2c91 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -274,7 +274,7 @@ namespace OpenSim.Region.Physics.Manager public virtual float Density { get; set; } public virtual float GravModifier { get; set; } public virtual float Friction { get; set; } - public virtual float Bounce { get; set; } + public virtual float Restitution { get; set; } /// /// Position of this actor. -- cgit v1.1 From 5b37063178d1a44e5db8ca6c8d47a12c011f8012 Mon Sep 17 00:00:00 2001 From: Melanie Date: Tue, 12 Mar 2013 03:47:27 +0100 Subject: Spot fix the interpenetration issue. Thanks, Ter. --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index f7e4c1c..bea34d4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1014,8 +1014,8 @@ namespace OpenSim.Region.Physics.OdePlugin offset.Y += contact.pos.Y; offset.Z += contact.pos.Z; - _position = offset; - return false; + //_position = offset; + //return false; } offset.X = contact.pos.X - _position.X; -- cgit v1.1 From 799ba5aa7b2b5d82e5e2f622929042b38da3809c Mon Sep 17 00:00:00 2001 From: teravus Date: Tue, 14 May 2013 19:17:31 -0400 Subject: * Tweaks the hard cut to apply to collisions of Greater then Normal Z 0.95. This fits within Ubit's framework of multi-body collisions, just moves the reactive force to the Midboxgeom(actual detection) instead of the bigbox geom(pre detection) --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 28 +++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index bea34d4..238e6e9 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1045,12 +1045,37 @@ namespace OpenSim.Region.Physics.OdePlugin if (me == midbox) { if (Math.Abs(contact.normal.Z) > 0.95f) + { offset.Z = contact.pos.Z - _position.Z; + offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth; + offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth; + offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth; + + if (reverse) + { + offset.X *= -contact.normal.X; + offset.Y *= -contact.normal.Y; + offset.Z *= -contact.normal.Z; + } + else + { + offset.X *= contact.normal.X; + offset.Y *= contact.normal.Y; + offset.Z *= contact.normal.Z; + } + + offset.X += contact.pos.X; + offset.Y += contact.pos.Y; + offset.Z += contact.pos.Z; + _position = offset; + return true; + } else offset.Z = contact.normal.Z; offset.Normalize(); + /* if (reverse) { contact.normal.X = offset.X; @@ -1063,7 +1088,8 @@ namespace OpenSim.Region.Physics.OdePlugin contact.normal.Y = -offset.Y; contact.normal.Z = -offset.Z; } - + */ + //_position.Z = offset.Z; return true; } -- cgit v1.1 From 477a5e3a35780809b1807e43f6deed8ede17f14d Mon Sep 17 00:00:00 2001 From: teravus Date: Tue, 14 May 2013 20:55:56 -0400 Subject: * This fixes the avatar stuck in objects on login and teleport by gently applying an upward motion when stuck in things on the Z * Comments describe how it filters out good, normal collisions, from 'stuck' collisions.. It's especially sensitive in feetbox collisions since this is where normal collisions happen under usual circumstances. --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 238e6e9..e912997 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -1097,8 +1097,20 @@ namespace OpenSim.Region.Physics.OdePlugin { float h = contact.pos.Z - _position.Z; + // Only do this if the normal is sufficiently pointing in the 'up' direction if (Math.Abs(contact.normal.Z) > 0.95f) { + // We Only want to do this if we're sunk into the object a bit and we're stuck and we're trying to move and feetcollision is false + if ((contact.depth > 0.0010f && _velocity.X == 0f && _velocity.Y == 0 && _velocity.Z == 0) + && (_target_velocity.X > 0 || _target_velocity.Y > 0 || _target_velocity.Z > 0) + && (!feetcollision) ) + { + m_collisionException = true; // Stop looping, do this only once not X times Contacts + _position.Z += contact.depth + 0.01f; // Move us Up the amount that we sank in, and add 0.01 meters to gently lift avatar up. + + return true; + } + if (contact.normal.Z > 0) contact.normal.Z = 1.0f; else -- cgit v1.1 From 4e72cf9ee21dd8833af860fa5af4fc91e11018cb Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 May 2013 02:08:14 +0100 Subject: *** DANGER TESTING **** changed prims mesh generation hopefully removing spurius faces. CHanged several aspects. Fixed prims inertia that was too low, still using box as model. Increased number of quickstep SOR iterations to 15. Keep it 15 even on heavy load ( will only jump simulation time). --- OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs | 4 +- OpenSim/Region/Physics/UbitMeshing/Mesh.cs | 33 +- OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs | 64 +- OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs | 1230 +++++--------------- .../Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 6 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 52 +- .../UbitOdePlugin/ODERayCastRequestManager.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 288 ++--- 9 files changed, 503 insertions(+), 1178 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs b/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs index 2938257..5dc1e78 100644 --- a/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs +++ b/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs @@ -256,9 +256,9 @@ public class Vertex : IComparable // settings your machine works with. Unusable for a machine readable file format :-( NumberFormatInfo nfi = new NumberFormatInfo(); nfi.NumberDecimalSeparator = "."; - nfi.NumberDecimalDigits = 3; + nfi.NumberDecimalDigits = 6; - String s1 = X.ToString("N2", nfi) + " " + Y.ToString("N2", nfi) + " " + Z.ToString("N2", nfi); + String s1 = X.ToString(nfi) + " " + Y.ToString(nfi) + " " + Z.ToString(nfi); return s1; } diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs index fa06926..0418893 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs @@ -205,34 +205,21 @@ namespace OpenSim.Region.Physics.Meshing } - private float fRound(float f) - { - int i; - if (f == 0f) - return f; - else if (f > 0f) - i = (int)(1e5f * f + 0.5f); - else - i = (int)(1e5f * f - 0.5f); - - return ((float)i * 1e-5f); - } - public void Add(Triangle triangle) { if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) throw new NotSupportedException("Attempt to Add to a pinned Mesh"); - // round down - triangle.v1.X = fRound(triangle.v1.X); - triangle.v1.Y = fRound(triangle.v1.Y); - triangle.v1.Z = fRound(triangle.v1.Z); - triangle.v2.X = fRound(triangle.v2.X); - triangle.v2.Y = fRound(triangle.v2.Y); - triangle.v2.Z = fRound(triangle.v2.Z); - triangle.v3.X = fRound(triangle.v3.X); - triangle.v3.Y = fRound(triangle.v3.Y); - triangle.v3.Z = fRound(triangle.v3.Z); + + triangle.v1.X = (float)Math.Round(triangle.v1.X, 6); + triangle.v1.Y = (float)Math.Round(triangle.v1.Y, 6); + triangle.v1.Z = (float)Math.Round(triangle.v1.Z, 6); + triangle.v2.X = (float)Math.Round(triangle.v2.X, 6); + triangle.v2.Y = (float)Math.Round(triangle.v2.Y, 6); + triangle.v2.Z = (float)Math.Round(triangle.v2.Z, 6); + triangle.v3.X = (float)Math.Round(triangle.v3.X, 6); + triangle.v3.Y = (float)Math.Round(triangle.v3.Y, 6); + triangle.v3.Z = (float)Math.Round(triangle.v3.Z, 6); if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs index 00cbfbd..c131c6f 100644 --- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs @@ -816,15 +816,31 @@ namespace OpenSim.Region.Physics.Meshing float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f; float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f; + + if (profileBegin < 0.0f) + profileBegin = 0.0f; + + if (profileEnd < 0.02f) + profileEnd = 0.02f; + else if (profileEnd > 1.0f) + profileEnd = 1.0f; + + if (profileBegin >= profileEnd) + profileBegin = profileEnd - 0.02f; + float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f; if (profileHollow > 0.95f) profileHollow = 0.95f; - + int sides = 4; LevelOfDetail iLOD = (LevelOfDetail)lod; - if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + byte profshape = (byte)(primShape.ProfileCurve & 0x07); + + if (profshape == (byte)ProfileShape.EquilateralTriangle + || profshape == (byte)ProfileShape.IsometricTriangle + || profshape == (byte)ProfileShape.RightTriangle) sides = 3; - else if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + else if (profshape == (byte)ProfileShape.Circle) { switch (iLOD) { @@ -835,7 +851,7 @@ namespace OpenSim.Region.Physics.Meshing default: sides = 24; break; } } - else if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + else if (profshape == (byte)ProfileShape.HalfCircle) { // half circle, prim is a sphere switch (iLOD) { @@ -865,10 +881,15 @@ namespace OpenSim.Region.Physics.Meshing else if (primShape.HollowShape == HollowShape.Square) hollowSides = 4; else if (primShape.HollowShape == HollowShape.Triangle) - hollowSides = 3; + { + if (profshape == (byte)ProfileShape.HalfCircle) + hollowSides = 6; + else + hollowSides = 3; + } primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides); - + if (primMesh.errorMessage != null) if (primMesh.errorMessage.Length > 0) m_log.Error("[ERROR] " + primMesh.errorMessage); @@ -880,17 +901,11 @@ namespace OpenSim.Region.Physics.Meshing if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible) { - primMesh.twistBegin = primShape.PathTwistBegin * 18 / 10; - primMesh.twistEnd = primShape.PathTwist * 18 / 10; + primMesh.twistBegin = (primShape.PathTwistBegin * 18) / 10; + primMesh.twistEnd = (primShape.PathTwist * 18) / 10; primMesh.taperX = pathScaleX; primMesh.taperY = pathScaleY; - if (profileBegin < 0.0f || profileBegin >= profileEnd || profileEnd > 1.0f) - { - ReportPrimError("*** CORRUPT PRIM!! ***", primName, primMesh); - if (profileBegin < 0.0f) profileBegin = 0.0f; - if (profileEnd > 1.0f) profileEnd = 1.0f; - } #if SPAM m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString()); #endif @@ -911,17 +926,11 @@ namespace OpenSim.Region.Physics.Meshing primMesh.radius = 0.01f * primShape.PathRadiusOffset; primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions; primMesh.skew = 0.01f * primShape.PathSkew; - primMesh.twistBegin = primShape.PathTwistBegin * 36 / 10; - primMesh.twistEnd = primShape.PathTwist * 36 / 10; + primMesh.twistBegin = (primShape.PathTwistBegin * 36) / 10; + primMesh.twistEnd = (primShape.PathTwist * 36) / 10; primMesh.taperX = primShape.PathTaperX * 0.01f; primMesh.taperY = primShape.PathTaperY * 0.01f; - if (profileBegin < 0.0f || profileBegin >= profileEnd || profileEnd > 1.0f) - { - ReportPrimError("*** CORRUPT PRIM!! ***", primName, primMesh); - if (profileBegin < 0.0f) profileBegin = 0.0f; - if (profileEnd > 1.0f) profileEnd = 1.0f; - } #if SPAM m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString()); #endif @@ -1031,14 +1040,19 @@ namespace OpenSim.Region.Physics.Meshing public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) { - return CreateMesh(primName, primShape, size, lod, false,false,false,false); + return CreateMesh(primName, primShape, size, lod, false,false,false); } public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { - return CreateMesh(primName, primShape, size, lod, false,false,false,false); + return CreateMesh(primName, primShape, size, lod, false,false,false); } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde) + { + return CreateMesh(primName, primShape, size, lod, false, false, false); + } + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) { Mesh mesh = null; @@ -1080,7 +1094,7 @@ namespace OpenSim.Region.Physics.Meshing private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f); - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) { #if SPAM m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName); diff --git a/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs index 4049ee1..8eb136b 100644 --- a/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs +++ b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs @@ -225,26 +225,6 @@ namespace PrimMesher } } - public struct UVCoord - { - public float U; - public float V; - - - public UVCoord(float u, float v) - { - this.U = u; - this.V = v; - } - - public UVCoord Flip() - { - this.U = 1.0f - this.U; - this.V = 1.0f - this.V; - return this; - } - } - public struct Face { public int primFace; @@ -254,16 +234,6 @@ namespace PrimMesher public int v2; public int v3; - //normals - public int n1; - public int n2; - public int n3; - - // uvs - public int uv1; - public int uv2; - public int uv3; - public Face(int v1, int v2, int v3) { primFace = 0; @@ -272,31 +242,6 @@ namespace PrimMesher this.v2 = v2; this.v3 = v3; - this.n1 = 0; - this.n2 = 0; - this.n3 = 0; - - this.uv1 = 0; - this.uv2 = 0; - this.uv3 = 0; - - } - - public Face(int v1, int v2, int v3, int n1, int n2, int n3) - { - primFace = 0; - - this.v1 = v1; - this.v2 = v2; - this.v3 = v3; - - this.n1 = n1; - this.n2 = n2; - this.n3 = n3; - - this.uv1 = 0; - this.uv2 = 0; - this.uv3 = 0; } public Coord SurfaceNormal(List coordList) @@ -312,96 +257,6 @@ namespace PrimMesher } } - public struct ViewerFace - { - public int primFaceNumber; - - public Coord v1; - public Coord v2; - public Coord v3; - - public int coordIndex1; - public int coordIndex2; - public int coordIndex3; - - public Coord n1; - public Coord n2; - public Coord n3; - - public UVCoord uv1; - public UVCoord uv2; - public UVCoord uv3; - - public ViewerFace(int primFaceNumber) - { - this.primFaceNumber = primFaceNumber; - - this.v1 = new Coord(); - this.v2 = new Coord(); - this.v3 = new Coord(); - - this.coordIndex1 = this.coordIndex2 = this.coordIndex3 = -1; // -1 means not assigned yet - - this.n1 = new Coord(); - this.n2 = new Coord(); - this.n3 = new Coord(); - - this.uv1 = new UVCoord(); - this.uv2 = new UVCoord(); - this.uv3 = new UVCoord(); - } - - public void Scale(float x, float y, float z) - { - this.v1.X *= x; - this.v1.Y *= y; - this.v1.Z *= z; - - this.v2.X *= x; - this.v2.Y *= y; - this.v2.Z *= z; - - this.v3.X *= x; - this.v3.Y *= y; - this.v3.Z *= z; - } - - public void AddPos(float x, float y, float z) - { - this.v1.X += x; - this.v2.X += x; - this.v3.X += x; - - this.v1.Y += y; - this.v2.Y += y; - this.v3.Y += y; - - this.v1.Z += z; - this.v2.Z += z; - this.v3.Z += z; - } - - public void AddRot(Quat q) - { - this.v1 *= q; - this.v2 *= q; - this.v3 *= q; - - this.n1 *= q; - this.n2 *= q; - this.n3 *= q; - } - - public void CalcSurfaceNormal() - { - - Coord edge1 = new Coord(this.v2.X - this.v1.X, this.v2.Y - this.v1.Y, this.v2.Z - this.v1.Z); - Coord edge2 = new Coord(this.v3.X - this.v1.X, this.v3.Y - this.v1.Y, this.v3.Z - this.v1.Z); - - this.n1 = this.n2 = this.n3 = Coord.Cross(edge1, edge2).Normalize(); - } - } - internal struct Angle { internal float angle; @@ -428,14 +283,6 @@ namespace PrimMesher new Angle(1.0f, 1.0f, 0.0f) }; - private static Coord[] normals3 = - { - new Coord(0.25f, 0.4330127019f, 0.0f).Normalize(), - new Coord(-0.5f, 0.0f, 0.0f).Normalize(), - new Coord(0.25f, -0.4330127019f, 0.0f).Normalize(), - new Coord(0.25f, 0.4330127019f, 0.0f).Normalize() - }; - private static Angle[] angles4 = { new Angle(0.0f, 1.0f, 0.0f), @@ -445,13 +292,32 @@ namespace PrimMesher new Angle(1.0f, 1.0f, 0.0f) }; - private static Coord[] normals4 = + private static Angle[] angles6 = { - new Coord(0.5f, 0.5f, 0.0f).Normalize(), - new Coord(-0.5f, 0.5f, 0.0f).Normalize(), - new Coord(-0.5f, -0.5f, 0.0f).Normalize(), - new Coord(0.5f, -0.5f, 0.0f).Normalize(), - new Coord(0.5f, 0.5f, 0.0f).Normalize() + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles12 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f), + new Angle(1.0f, 1.0f, 0.0f) }; private static Angle[] angles24 = @@ -503,56 +369,72 @@ namespace PrimMesher } internal List angles; - internal List normals; - internal void makeAngles(int sides, float startAngle, float stopAngle) + internal void makeAngles(int sides, float startAngle, float stopAngle, bool hasCut) { angles = new List(); - normals = new List(); - double twoPi = System.Math.PI * 2.0; - float twoPiInv = 1.0f / (float)twoPi; + const double twoPi = System.Math.PI * 2.0; + const float twoPiInv = (float)(1.0d / twoPi); if (sides < 1) throw new Exception("number of sides not greater than zero"); if (stopAngle <= startAngle) throw new Exception("stopAngle not greater than startAngle"); - if ((sides == 3 || sides == 4 || sides == 24)) + if ((sides == 3 || sides == 4 || sides == 6 || sides == 12 || sides == 24)) { startAngle *= twoPiInv; stopAngle *= twoPiInv; Angle[] sourceAngles; - if (sides == 3) - sourceAngles = angles3; - else if (sides == 4) - sourceAngles = angles4; - else sourceAngles = angles24; + switch (sides) + { + case 3: + sourceAngles = angles3; + break; + case 4: + sourceAngles = angles4; + break; + case 6: + sourceAngles = angles6; + break; + case 12: + sourceAngles = angles12; + break; + default: + sourceAngles = angles24; + break; + } int startAngleIndex = (int)(startAngle * sides); int endAngleIndex = sourceAngles.Length - 1; - if (stopAngle < 1.0f) - endAngleIndex = (int)(stopAngle * sides) + 1; - if (endAngleIndex == startAngleIndex) - endAngleIndex++; - for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++) + if (hasCut) { - angles.Add(sourceAngles[angleIndex]); - if (sides == 3) - normals.Add(normals3[angleIndex]); - else if (sides == 4) - normals.Add(normals4[angleIndex]); - } + if (stopAngle < 1.0f) + endAngleIndex = (int)(stopAngle * sides) + 1; + if (endAngleIndex == startAngleIndex) + endAngleIndex++; + + for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++) + { + angles.Add(sourceAngles[angleIndex]); + } - if (startAngle > 0.0f) - angles[0] = interpolatePoints(startAngle, angles[0], angles[1]); + if (startAngle > 0.0f) + angles[0] = interpolatePoints(startAngle, angles[0], angles[1]); - if (stopAngle < 1.0f) + if (stopAngle < 1.0f) + { + int lastAngleIndex = angles.Count - 1; + angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]); + } + } + else { - int lastAngleIndex = angles.Count - 1; - angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]); + for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex; angleIndex++) + angles.Add(sourceAngles[angleIndex]); } } else @@ -618,20 +500,10 @@ namespace PrimMesher public List coords; public List faces; - public List vertexNormals; - public List us; - public List faceUVs; - public List faceNumbers; // use these for making individual meshes for each prim face public List outerCoordIndices = null; public List hollowCoordIndices = null; - public List cut1CoordIndices = null; - public List cut2CoordIndices = null; - - public Coord faceNormal = new Coord(0.0f, 0.0f, 1.0f); - public Coord cutNormal1 = new Coord(); - public Coord cutNormal2 = new Coord(); public int numOuterVerts = 0; public int numHollowVerts = 0; @@ -639,7 +511,6 @@ namespace PrimMesher public int outerFaceNumber = -1; public int hollowFaceNumber = -1; - public bool calcVertexNormals = false; public int bottomFaceNumber = 0; public int numPrimFaces = 0; @@ -647,40 +518,19 @@ namespace PrimMesher { this.coords = new List(); this.faces = new List(); - this.vertexNormals = new List(); - this.us = new List(); - this.faceUVs = new List(); - this.faceNumbers = new List(); } - public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool createFaces, bool calcVertexNormals) + public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool hasProfileCut, bool createFaces) { - this.calcVertexNormals = calcVertexNormals; + const float halfSqr2 = 0.7071067811866f; + this.coords = new List(); this.faces = new List(); - this.vertexNormals = new List(); - this.us = new List(); - this.faceUVs = new List(); - this.faceNumbers = new List(); - - Coord center = new Coord(0.0f, 0.0f, 0.0f); List hollowCoords = new List(); - List hollowNormals = new List(); - List hollowUs = new List(); - - if (calcVertexNormals) - { - this.outerCoordIndices = new List(); - this.hollowCoordIndices = new List(); - this.cut1CoordIndices = new List(); - this.cut2CoordIndices = new List(); - } bool hasHollow = (hollow > 0.0f); - bool hasProfileCut = (profileStart > 0.0f || profileEnd < 1.0f); - AngleList angles = new AngleList(); AngleList hollowAngles = new AngleList(); @@ -688,14 +538,14 @@ namespace PrimMesher float yScale = 0.5f; if (sides == 4) // corners of a square are sqrt(2) from center { - xScale = 0.707107f; - yScale = 0.707107f; + xScale = halfSqr2; + yScale = halfSqr2; } float startAngle = profileStart * twoPi; float stopAngle = profileEnd * twoPi; - try { angles.makeAngles(sides, startAngle, stopAngle); } + try { angles.makeAngles(sides, startAngle, stopAngle,hasProfileCut); } catch (Exception ex) { @@ -707,6 +557,9 @@ namespace PrimMesher this.numOuterVerts = angles.angles.Count; + Angle angle; + Coord newVert = new Coord(); + // flag to create as few triangles as possible for 3 or 4 side profile bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut); @@ -716,7 +569,7 @@ namespace PrimMesher hollowAngles = angles; else { - try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle); } + try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle, hasProfileCut); } catch (Exception ex) { errorMessage = "makeAngles failed: Exception: " + ex.ToString() @@ -724,116 +577,48 @@ namespace PrimMesher return; } + + int numHollowAngles = hollowAngles.angles.Count; + for (int i = 0; i < numHollowAngles; i++) + { + angle = hollowAngles.angles[i]; + newVert.X = hollow * xScale * angle.X; + newVert.Y = hollow * yScale * angle.Y; + newVert.Z = 0.0f; + + hollowCoords.Add(newVert); + } } this.numHollowVerts = hollowAngles.angles.Count; } else if (!simpleFace) { + Coord center = new Coord(0.0f, 0.0f, 0.0f); this.coords.Add(center); - if (this.calcVertexNormals) - this.vertexNormals.Add(new Coord(0.0f, 0.0f, 1.0f)); - this.us.Add(0.0f); - } - - float z = 0.0f; - - Angle angle; - Coord newVert = new Coord(); - if (hasHollow && hollowSides != sides) - { - int numHollowAngles = hollowAngles.angles.Count; - for (int i = 0; i < numHollowAngles; i++) - { - angle = hollowAngles.angles[i]; - newVert.X = hollow * xScale * angle.X; - newVert.Y = hollow * yScale * angle.Y; - newVert.Z = z; - - hollowCoords.Add(newVert); - if (this.calcVertexNormals) - { - if (hollowSides < 5) - hollowNormals.Add(hollowAngles.normals[i].Invert()); - else - hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f)); - - if (hollowSides == 4) - hollowUs.Add(angle.angle * hollow * 0.707107f); - else - hollowUs.Add(angle.angle * hollow); - } - } } - int index = 0; int numAngles = angles.angles.Count; + bool hollowsame = (hasHollow && hollowSides == sides); for (int i = 0; i < numAngles; i++) { angle = angles.angles[i]; newVert.X = angle.X * xScale; newVert.Y = angle.Y * yScale; - newVert.Z = z; + newVert.Z = 0.0f; this.coords.Add(newVert); - if (this.calcVertexNormals) + if (hollowsame) { - this.outerCoordIndices.Add(this.coords.Count - 1); - - if (sides < 5) - { - this.vertexNormals.Add(angles.normals[i]); - float u = angle.angle; - this.us.Add(u); - } - else - { - this.vertexNormals.Add(new Coord(angle.X, angle.Y, 0.0f)); - this.us.Add(angle.angle); - } - } - - if (hasHollow) - { - if (hollowSides == sides) - { - newVert.X *= hollow; - newVert.Y *= hollow; - newVert.Z = z; - hollowCoords.Add(newVert); - if (this.calcVertexNormals) - { - if (sides < 5) - { - hollowNormals.Add(angles.normals[i].Invert()); - } - - else - hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f)); - - hollowUs.Add(angle.angle * hollow); - } - } - } - else if (!simpleFace && createFaces && angle.angle > 0.0001f) - { - Face newFace = new Face(); - newFace.v1 = 0; - newFace.v2 = index; - newFace.v3 = index + 1; - - this.faces.Add(newFace); + newVert.X *= hollow; + newVert.Y *= hollow; + hollowCoords.Add(newVert); } - index += 1; } if (hasHollow) { hollowCoords.Reverse(); - if (this.calcVertexNormals) - { - hollowNormals.Reverse(); - hollowUs.Reverse(); - } + this.coords.AddRange(hollowCoords); if (createFaces) { @@ -855,187 +640,176 @@ namespace PrimMesher newFace.v3 = numTotalVerts - coordIndex - 1; this.faces.Add(newFace); } + if (!hasProfileCut) + { + newFace.v1 = this.numOuterVerts - 1; + newFace.v2 = 0; + newFace.v3 = this.numOuterVerts; + this.faces.Add(newFace); + + newFace.v1 = 0; + newFace.v2 = numTotalVerts - 1; + newFace.v3 = this.numOuterVerts; + this.faces.Add(newFace); + } } - else + else if (this.numOuterVerts < this.numHollowVerts) { - if (this.numOuterVerts < this.numHollowVerts) + Face newFace = new Face(); + int j = 0; // j is the index for outer vertices + int i; + int maxJ = this.numOuterVerts - 1; + float curHollowAngle = 0; + for (i = 0; i < this.numHollowVerts; i++) // i is the index for inner vertices { - Face newFace = new Face(); - int j = 0; // j is the index for outer vertices - int maxJ = this.numOuterVerts - 1; - for (int i = 0; i < this.numHollowVerts; i++) // i is the index for inner vertices + curHollowAngle = hollowAngles.angles[i].angle; + if (j < maxJ) { - if (j < maxJ) - if (angles.angles[j + 1].angle - hollowAngles.angles[i].angle < hollowAngles.angles[i].angle - angles.angles[j].angle + 0.000001f) - { - newFace.v1 = numTotalVerts - i - 1; - newFace.v2 = j; - newFace.v3 = j + 1; + if (angles.angles[j + 1].angle - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f) + { + newFace.v1 = numTotalVerts - i - 1; + newFace.v2 = j; + newFace.v3 = j + 1; + this.faces.Add(newFace); + j++; + } + } + else + { + if (1.0f - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f) + break; + } + + newFace.v1 = j; + newFace.v2 = numTotalVerts - i - 2; + newFace.v3 = numTotalVerts - i - 1; - this.faces.Add(newFace); - j += 1; - } + this.faces.Add(newFace); + } - newFace.v1 = j; - newFace.v2 = numTotalVerts - i - 2; - newFace.v3 = numTotalVerts - i - 1; + if (!hasProfileCut) + { + if (i == this.numHollowVerts) + { + newFace.v1 = numTotalVerts - this.numHollowVerts; + newFace.v2 = maxJ; + newFace.v3 = 0; this.faces.Add(newFace); } - } - else // numHollowVerts < numOuterVerts - { - Face newFace = new Face(); - int j = 0; // j is the index for inner vertices - int maxJ = this.numHollowVerts - 1; - for (int i = 0; i < this.numOuterVerts; i++) + else { - if (j < maxJ) - if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f) - { - newFace.v1 = i; - newFace.v2 = numTotalVerts - j - 2; - newFace.v3 = numTotalVerts - j - 1; + if (1.0f - curHollowAngle < curHollowAngle - angles.angles[maxJ].angle + 0.000001f) + { + newFace.v1 = numTotalVerts - i - 1; + newFace.v2 = maxJ; + newFace.v3 = 0; - this.faces.Add(newFace); - j += 1; - } + this.faces.Add(newFace); + } - newFace.v1 = numTotalVerts - j - 1; - newFace.v2 = i; - newFace.v3 = i + 1; + for (; i < this.numHollowVerts - 1; i++) + { + newFace.v1 = 0; + newFace.v2 = numTotalVerts - i - 2; + newFace.v3 = numTotalVerts - i - 1; - this.faces.Add(newFace); + this.faces.Add(newFace); + } } + + newFace.v1 = 0; + newFace.v2 = numTotalVerts - this.numHollowVerts; + newFace.v3 = numTotalVerts - 1; + this.faces.Add(newFace); } } - } - - if (calcVertexNormals) - { - foreach (Coord hc in hollowCoords) + else // numHollowVerts < numOuterVerts { - this.coords.Add(hc); - hollowCoordIndices.Add(this.coords.Count - 1); - } - } - else - this.coords.AddRange(hollowCoords); + Face newFace = new Face(); + int j = 0; // j is the index for inner vertices + int maxJ = this.numHollowVerts - 1; + for (int i = 0; i < this.numOuterVerts; i++) + { + if (j < maxJ) + if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f) + { + newFace.v1 = i; + newFace.v2 = numTotalVerts - j - 2; + newFace.v3 = numTotalVerts - j - 1; - if (this.calcVertexNormals) - { - this.vertexNormals.AddRange(hollowNormals); - this.us.AddRange(hollowUs); + this.faces.Add(newFace); + j += 1; + } - } - } + newFace.v1 = numTotalVerts - j - 1; + newFace.v2 = i; + newFace.v3 = i + 1; - if (simpleFace && createFaces) - { - if (sides == 3) - this.faces.Add(new Face(0, 1, 2)); - else if (sides == 4) - { - this.faces.Add(new Face(0, 1, 2)); - this.faces.Add(new Face(0, 2, 3)); - } - } + this.faces.Add(newFace); + } - if (calcVertexNormals && hasProfileCut) - { - int lastOuterVertIndex = this.numOuterVerts - 1; + if (!hasProfileCut) + { + int i = this.numOuterVerts - 1; - if (hasHollow) - { - this.cut1CoordIndices.Add(0); - this.cut1CoordIndices.Add(this.coords.Count - 1); + if (hollowAngles.angles[0].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[maxJ].angle + 0.000001f) + { + newFace.v1 = 0; + newFace.v2 = numTotalVerts - maxJ - 1; + newFace.v3 = numTotalVerts - 1; - this.cut2CoordIndices.Add(lastOuterVertIndex + 1); - this.cut2CoordIndices.Add(lastOuterVertIndex); + this.faces.Add(newFace); + } - this.cutNormal1.X = this.coords[0].Y - this.coords[this.coords.Count - 1].Y; - this.cutNormal1.Y = -(this.coords[0].X - this.coords[this.coords.Count - 1].X); + newFace.v1 = numTotalVerts - maxJ - 1; + newFace.v2 = i; + newFace.v3 = 0; - this.cutNormal2.X = this.coords[lastOuterVertIndex + 1].Y - this.coords[lastOuterVertIndex].Y; - this.cutNormal2.Y = -(this.coords[lastOuterVertIndex + 1].X - this.coords[lastOuterVertIndex].X); + this.faces.Add(newFace); + } + } } + + } + else if (createFaces) + { + if (simpleFace) + { + if (sides == 3) + this.faces.Add(new Face(0, 1, 2)); + else if (sides == 4) + { + this.faces.Add(new Face(0, 1, 2)); + this.faces.Add(new Face(0, 2, 3)); + } + } else { - this.cut1CoordIndices.Add(0); - this.cut1CoordIndices.Add(1); - - this.cut2CoordIndices.Add(lastOuterVertIndex); - this.cut2CoordIndices.Add(0); - - this.cutNormal1.X = this.vertexNormals[1].Y; - this.cutNormal1.Y = -this.vertexNormals[1].X; - - this.cutNormal2.X = -this.vertexNormals[this.vertexNormals.Count - 2].Y; - this.cutNormal2.Y = this.vertexNormals[this.vertexNormals.Count - 2].X; - + for (int i = 1; i < numAngles ; i++) + { + Face newFace = new Face(); + newFace.v1 = 0; + newFace.v2 = i; + newFace.v3 = i + 1; + this.faces.Add(newFace); + } + if (!hasProfileCut) + { + Face newFace = new Face(); + newFace.v1 = 0; + newFace.v2 = numAngles; + newFace.v3 = 1; + this.faces.Add(newFace); + } } - this.cutNormal1.Normalize(); - this.cutNormal2.Normalize(); } - this.MakeFaceUVs(); hollowCoords = null; - hollowNormals = null; - hollowUs = null; - - if (calcVertexNormals) - { // calculate prim face numbers - - // face number order is top, outer, hollow, bottom, start cut, end cut - // I know it's ugly but so is the whole concept of prim face numbers - - int faceNum = 1; // start with outer faces - this.outerFaceNumber = faceNum; - - int startVert = hasProfileCut && !hasHollow ? 1 : 0; - if (startVert > 0) - this.faceNumbers.Add(-1); - for (int i = 0; i < this.numOuterVerts - 1; i++) - this.faceNumbers.Add(sides < 5 && i <= sides ? faceNum++ : faceNum); - - this.faceNumbers.Add(hasProfileCut ? -1 : faceNum++); - - if (sides > 4 && (hasHollow || hasProfileCut)) - faceNum++; - - if (sides < 5 && (hasHollow || hasProfileCut) && this.numOuterVerts < sides) - faceNum++; - - if (hasHollow) - { - for (int i = 0; i < this.numHollowVerts; i++) - this.faceNumbers.Add(faceNum); - - this.hollowFaceNumber = faceNum++; - } - - this.bottomFaceNumber = faceNum++; - - if (hasHollow && hasProfileCut) - this.faceNumbers.Add(faceNum++); - - for (int i = 0; i < this.faceNumbers.Count; i++) - if (this.faceNumbers[i] == -1) - this.faceNumbers[i] = faceNum++; - - this.numPrimFaces = faceNum; - } - } - public void MakeFaceUVs() - { - this.faceUVs = new List(); - foreach (Coord c in this.coords) - this.faceUVs.Add(new UVCoord(1.0f - (0.5f + c.X), 1.0f - (0.5f - c.Y))); - } public Profile Copy() { @@ -1047,24 +821,10 @@ namespace PrimMesher Profile copy = new Profile(); copy.coords.AddRange(this.coords); - copy.faceUVs.AddRange(this.faceUVs); if (needFaces) copy.faces.AddRange(this.faces); - if ((copy.calcVertexNormals = this.calcVertexNormals) == true) - { - copy.vertexNormals.AddRange(this.vertexNormals); - copy.faceNormal = this.faceNormal; - copy.cutNormal1 = this.cutNormal1; - copy.cutNormal2 = this.cutNormal2; - copy.us.AddRange(this.us); - copy.faceNumbers.AddRange(this.faceNumbers); - - copy.cut1CoordIndices = new List(this.cut1CoordIndices); - copy.cut2CoordIndices = new List(this.cut2CoordIndices); - copy.hollowCoordIndices = new List(this.hollowCoordIndices); - copy.outerCoordIndices = new List(this.outerCoordIndices); - } + copy.numOuterVerts = this.numOuterVerts; copy.numHollowVerts = this.numHollowVerts; @@ -1099,18 +859,6 @@ namespace PrimMesher for (i = 0; i < numVerts; i++) this.coords[i] *= q; - - if (this.calcVertexNormals) - { - int numNormals = this.vertexNormals.Count; - for (i = 0; i < numNormals; i++) - this.vertexNormals[i] *= q; - - this.faceNormal *= q; - this.cutNormal1 *= q; - this.cutNormal2 *= q; - - } } public void Scale(float x, float y) @@ -1146,29 +894,6 @@ namespace PrimMesher tmpFace.v1 = tmp; this.faces[i] = tmpFace; } - - if (this.calcVertexNormals) - { - int normalCount = this.vertexNormals.Count; - if (normalCount > 0) - { - Coord n = this.vertexNormals[normalCount - 1]; - n.Z = -n.Z; - this.vertexNormals[normalCount - 1] = n; - } - } - - this.faceNormal.X = -this.faceNormal.X; - this.faceNormal.Y = -this.faceNormal.Y; - this.faceNormal.Z = -this.faceNormal.Z; - - int numfaceUVs = this.faceUVs.Count; - for (i = 0; i < numfaceUVs; i++) - { - UVCoord uv = this.faceUVs[i]; - uv.V = 1.0f - uv.V; - this.faceUVs[i] = uv; - } } public void AddValue2FaceVertexIndices(int num) @@ -1186,25 +911,7 @@ namespace PrimMesher } } - public void AddValue2FaceNormalIndices(int num) - { - if (this.calcVertexNormals) - { - int numFaces = this.faces.Count; - Face tmpFace; - for (int i = 0; i < numFaces; i++) - { - tmpFace = this.faces[i]; - tmpFace.n1 += num; - tmpFace.n2 += num; - tmpFace.n3 += num; - - this.faces[i] = tmpFace; - } - } - } - - public void DumpRaw(String path, String name, String title) + public void DumpRaw(String path, String name, String title) { if (path == null) return; @@ -1451,11 +1158,9 @@ namespace PrimMesher private const float twoPi = 2.0f * (float)Math.PI; public List coords; - public List normals; +// public List normals; public List faces; - public List viewerFaces; - private int sides = 4; private int hollowSides = 4; private float profileStart = 0.0f; @@ -1478,15 +1183,8 @@ namespace PrimMesher public float revolutions = 1.0f; public int stepsPerRevolution = 24; - private int profileOuterFaceNumber = -1; - private int profileHollowFaceNumber = -1; - private bool hasProfileCut = false; private bool hasHollow = false; - public bool calcVertexNormals = false; - private bool normalsProcessed = false; - public bool viewerMode = false; - public bool sphereMode = false; public int numPrimFaces = 0; @@ -1518,27 +1216,16 @@ namespace PrimMesher s += "\nradius...............: " + this.radius.ToString(); s += "\nrevolutions..........: " + this.revolutions.ToString(); s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString(); - s += "\nsphereMode...........: " + this.sphereMode.ToString(); s += "\nhasProfileCut........: " + this.hasProfileCut.ToString(); - s += "\nhasHollow............: " + this.hasHollow.ToString(); - s += "\nviewerMode...........: " + this.viewerMode.ToString(); + s += "\nhasHollow............: " + this.hasHollow.ToString(); return s; } - public int ProfileOuterFaceNumber - { - get { return profileOuterFaceNumber; } - } - - public int ProfileHollowFaceNumber - { - get { return profileHollowFaceNumber; } - } - public bool HasProfileCut { get { return hasProfileCut; } + set { hasProfileCut = value; } } public bool HasHollow @@ -1555,6 +1242,7 @@ namespace PrimMesher /// /// /// + /// public PrimMesh(int sides, float profileStart, float profileEnd, float hollow, int hollowSides) { this.coords = new List(); @@ -1594,32 +1282,12 @@ namespace PrimMesher this.coords = new List(); this.faces = new List(); - if (this.viewerMode) - { - this.viewerFaces = new List(); - this.calcVertexNormals = true; - } - - if (this.calcVertexNormals) - this.normals = new List(); - int steps = 1; float length = this.pathCutEnd - this.pathCutBegin; - normalsProcessed = false; - if (this.viewerMode && this.sides == 3) - { - // prisms don't taper well so add some vertical resolution - // other prims may benefit from this but just do prisms for now - if (Math.Abs(this.taperX) > 0.01 || Math.Abs(this.taperY) > 0.01) - steps = (int)(steps * 4.5 * length); - } + this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; - if (this.sphereMode) - this.hasProfileCut = this.profileEnd - this.profileStart < 0.4999f; - else - this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; this.hasHollow = (this.hollow > 0.001f); float twistBegin = this.twistBegin / 360.0f * twoPi; @@ -1701,47 +1369,16 @@ namespace PrimMesher hollow *= 1.414f; } - Profile profile = new Profile(this.sides, this.profileStart, this.profileEnd, hollow, this.hollowSides, true, calcVertexNormals); + Profile profile = new Profile(this.sides, this.profileStart, this.profileEnd, hollow, this.hollowSides, this.hasProfileCut,true); this.errorMessage = profile.errorMessage; this.numPrimFaces = profile.numPrimFaces; - int cut1FaceNumber = profile.bottomFaceNumber + 1; - int cut2FaceNumber = cut1FaceNumber + 1; - if (!needEndFaces) - { - cut1FaceNumber -= 2; - cut2FaceNumber -= 2; - } - - profileOuterFaceNumber = profile.outerFaceNumber; - if (!needEndFaces) - profileOuterFaceNumber--; - - if (hasHollow) - { - profileHollowFaceNumber = profile.hollowFaceNumber; - if (!needEndFaces) - profileHollowFaceNumber--; - } - - int cut1Vert = -1; - int cut2Vert = -1; - if (hasProfileCut) - { - cut1Vert = hasHollow ? profile.coords.Count - 1 : 0; - cut2Vert = hasHollow ? profile.numOuterVerts - 1 : profile.numOuterVerts; - } - if (initialProfileRot != 0.0f) { profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot)); - if (viewerMode) - profile.MakeFaceUVs(); } - Coord lastCutNormal1 = new Coord(); - Coord lastCutNormal2 = new Coord(); float thisV = 0.0f; float lastV = 0.0f; @@ -1764,57 +1401,21 @@ namespace PrimMesher path.stepsPerRevolution = stepsPerRevolution; path.Create(pathType, steps); + + int lastNode = path.pathNodes.Count -1; for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) { PathNode node = path.pathNodes[nodeIndex]; Profile newLayer = profile.Copy(); - newLayer.Scale(node.xScale, node.yScale); + newLayer.Scale(node.xScale, node.yScale); newLayer.AddRot(node.rotation); newLayer.AddPos(node.position); if (needEndFaces && nodeIndex == 0) { newLayer.FlipNormals(); - - // add the bottom faces to the viewerFaces list - if (this.viewerMode) - { - Coord faceNormal = newLayer.faceNormal; - ViewerFace newViewerFace = new ViewerFace(profile.bottomFaceNumber); - int numFaces = newLayer.faces.Count; - List faces = newLayer.faces; - - for (int i = 0; i < numFaces; i++) - { - Face face = faces[i]; - newViewerFace.v1 = newLayer.coords[face.v1]; - newViewerFace.v2 = newLayer.coords[face.v2]; - newViewerFace.v3 = newLayer.coords[face.v3]; - - newViewerFace.coordIndex1 = face.v1; - newViewerFace.coordIndex2 = face.v2; - newViewerFace.coordIndex3 = face.v3; - - newViewerFace.n1 = faceNormal; - newViewerFace.n2 = faceNormal; - newViewerFace.n3 = faceNormal; - - newViewerFace.uv1 = newLayer.faceUVs[face.v1]; - newViewerFace.uv2 = newLayer.faceUVs[face.v2]; - newViewerFace.uv3 = newLayer.faceUVs[face.v3]; - - if (pathType == PathType.Linear) - { - newViewerFace.uv1.Flip(); - newViewerFace.uv2.Flip(); - newViewerFace.uv3.Flip(); - } - - this.viewerFaces.Add(newViewerFace); - } - } } // if (nodeIndex == 0) // append this layer @@ -1824,15 +1425,17 @@ namespace PrimMesher this.coords.AddRange(newLayer.coords); - if (this.calcVertexNormals) + if (needEndFaces) { - newLayer.AddValue2FaceNormalIndices(this.normals.Count); - this.normals.AddRange(newLayer.vertexNormals); + if (nodeIndex == 0) + this.faces.AddRange(newLayer.faces); + else if (nodeIndex == lastNode) + { + if (node.xScale > 1e-6 && node.yScale > 1e-6) + this.faces.AddRange(newLayer.faces); + } } - if (node.percentOfPath < this.pathCutBegin + 0.01f || node.percentOfPath > this.pathCutEnd - 0.01f) - this.faces.AddRange(newLayer.faces); - // fill faces between layers int numVerts = newLayer.coords.Count; @@ -1843,219 +1446,88 @@ namespace PrimMesher if (nodeIndex > 0) { - int startVert = coordsLen + 1; + int startVert = coordsLen; int endVert = this.coords.Count; - - if (sides < 5 || this.hasProfileCut || this.hasHollow) - startVert--; - - for (int i = startVert; i < endVert; i++) + if (!this.hasProfileCut) { - int iNext = i + 1; - if (i == endVert - 1) - iNext = startVert; - - int whichVert = i - startVert; + int i = startVert; + for (int l = 0; l < profile.numOuterVerts - 1; l++) + { + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = i + 1; + this.faces.Add(newFace1); + + newFace2.v1 = i + 1; + newFace2.v2 = i - numVerts; + newFace2.v3 = i + 1 - numVerts; + this.faces.Add(newFace2); + i++; + } newFace1.v1 = i; newFace1.v2 = i - numVerts; - newFace1.v3 = iNext; - - newFace1.n1 = newFace1.v1; - newFace1.n2 = newFace1.v2; - newFace1.n3 = newFace1.v3; + newFace1.v3 = startVert; this.faces.Add(newFace1); - newFace2.v1 = iNext; + newFace2.v1 = startVert; newFace2.v2 = i - numVerts; - newFace2.v3 = iNext - numVerts; - - newFace2.n1 = newFace2.v1; - newFace2.n2 = newFace2.v2; - newFace2.n3 = newFace2.v3; + newFace2.v3 = startVert - numVerts; this.faces.Add(newFace2); - if (this.viewerMode) + if (this.hasHollow) { - // add the side faces to the list of viewerFaces here - - int primFaceNum = profile.faceNumbers[whichVert]; - if (!needEndFaces) - primFaceNum -= 1; - - ViewerFace newViewerFace1 = new ViewerFace(primFaceNum); - ViewerFace newViewerFace2 = new ViewerFace(primFaceNum); - - int uIndex = whichVert; - if (!hasHollow && sides > 4 && uIndex < newLayer.us.Count - 1) - { - uIndex++; - } - - float u1 = newLayer.us[uIndex]; - float u2 = 1.0f; - if (uIndex < (int)newLayer.us.Count - 1) - u2 = newLayer.us[uIndex + 1]; - - if (whichVert == cut1Vert || whichVert == cut2Vert) - { - u1 = 0.0f; - u2 = 1.0f; - } - else if (sides < 5) - { - if (whichVert < profile.numOuterVerts) - { // boxes and prisms have one texture face per side of the prim, so the U values have to be scaled - // to reflect the entire texture width - u1 *= sides; - u2 *= sides; - u2 -= (int)u1; - u1 -= (int)u1; - if (u2 < 0.1f) - u2 = 1.0f; - } - } - - if (this.sphereMode) + startVert = ++i; + for (int l = 0; l < profile.numHollowVerts - 1; l++) { - if (whichVert != cut1Vert && whichVert != cut2Vert) - { - u1 = u1 * 2.0f - 1.0f; - u2 = u2 * 2.0f - 1.0f; - - if (whichVert >= newLayer.numOuterVerts) - { - u1 -= hollow; - u2 -= hollow; - } - - } - } - - newViewerFace1.uv1.U = u1; - newViewerFace1.uv2.U = u1; - newViewerFace1.uv3.U = u2; - - newViewerFace1.uv1.V = thisV; - newViewerFace1.uv2.V = lastV; - newViewerFace1.uv3.V = thisV; - - newViewerFace2.uv1.U = u2; - newViewerFace2.uv2.U = u1; - newViewerFace2.uv3.U = u2; - - newViewerFace2.uv1.V = thisV; - newViewerFace2.uv2.V = lastV; - newViewerFace2.uv3.V = lastV; - - newViewerFace1.v1 = this.coords[newFace1.v1]; - newViewerFace1.v2 = this.coords[newFace1.v2]; - newViewerFace1.v3 = this.coords[newFace1.v3]; - - newViewerFace2.v1 = this.coords[newFace2.v1]; - newViewerFace2.v2 = this.coords[newFace2.v2]; - newViewerFace2.v3 = this.coords[newFace2.v3]; - - newViewerFace1.coordIndex1 = newFace1.v1; - newViewerFace1.coordIndex2 = newFace1.v2; - newViewerFace1.coordIndex3 = newFace1.v3; - - newViewerFace2.coordIndex1 = newFace2.v1; - newViewerFace2.coordIndex2 = newFace2.v2; - newViewerFace2.coordIndex3 = newFace2.v3; - - // profile cut faces - if (whichVert == cut1Vert) - { - newViewerFace1.primFaceNumber = cut1FaceNumber; - newViewerFace2.primFaceNumber = cut1FaceNumber; - newViewerFace1.n1 = newLayer.cutNormal1; - newViewerFace1.n2 = newViewerFace1.n3 = lastCutNormal1; - - newViewerFace2.n1 = newViewerFace2.n3 = newLayer.cutNormal1; - newViewerFace2.n2 = lastCutNormal1; - } - else if (whichVert == cut2Vert) - { - newViewerFace1.primFaceNumber = cut2FaceNumber; - newViewerFace2.primFaceNumber = cut2FaceNumber; - newViewerFace1.n1 = newLayer.cutNormal2; - newViewerFace1.n2 = lastCutNormal2; - newViewerFace1.n3 = lastCutNormal2; - - newViewerFace2.n1 = newLayer.cutNormal2; - newViewerFace2.n3 = newLayer.cutNormal2; - newViewerFace2.n2 = lastCutNormal2; - } - - else // outer and hollow faces - { - if ((sides < 5 && whichVert < newLayer.numOuterVerts) || (hollowSides < 5 && whichVert >= newLayer.numOuterVerts)) - { // looks terrible when path is twisted... need vertex normals here - newViewerFace1.CalcSurfaceNormal(); - newViewerFace2.CalcSurfaceNormal(); - } - else - { - newViewerFace1.n1 = this.normals[newFace1.n1]; - newViewerFace1.n2 = this.normals[newFace1.n2]; - newViewerFace1.n3 = this.normals[newFace1.n3]; - - newViewerFace2.n1 = this.normals[newFace2.n1]; - newViewerFace2.n2 = this.normals[newFace2.n2]; - newViewerFace2.n3 = this.normals[newFace2.n3]; - } + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = i + 1; + this.faces.Add(newFace1); + + newFace2.v1 = i + 1; + newFace2.v2 = i - numVerts; + newFace2.v3 = i + 1 - numVerts; + this.faces.Add(newFace2); + i++; } - this.viewerFaces.Add(newViewerFace1); - this.viewerFaces.Add(newViewerFace2); + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = startVert; + this.faces.Add(newFace1); + newFace2.v1 = startVert; + newFace2.v2 = i - numVerts; + newFace2.v3 = startVert - numVerts; + this.faces.Add(newFace2); } - } - } - lastCutNormal1 = newLayer.cutNormal1; - lastCutNormal2 = newLayer.cutNormal2; - lastV = thisV; - if (needEndFaces && nodeIndex == path.pathNodes.Count - 1 && viewerMode) - { - // add the top faces to the viewerFaces list here - Coord faceNormal = newLayer.faceNormal; - ViewerFace newViewerFace = new ViewerFace(0); - int numFaces = newLayer.faces.Count; - List faces = newLayer.faces; - - for (int i = 0; i < numFaces; i++) + } + else { - Face face = faces[i]; - newViewerFace.v1 = newLayer.coords[face.v1 - coordsLen]; - newViewerFace.v2 = newLayer.coords[face.v2 - coordsLen]; - newViewerFace.v3 = newLayer.coords[face.v3 - coordsLen]; - - newViewerFace.coordIndex1 = face.v1 - coordsLen; - newViewerFace.coordIndex2 = face.v2 - coordsLen; - newViewerFace.coordIndex3 = face.v3 - coordsLen; + for (int i = startVert; i < endVert; i++) + { + int iNext = i + 1; + if (i == endVert - 1) + iNext = startVert; - newViewerFace.n1 = faceNormal; - newViewerFace.n2 = faceNormal; - newViewerFace.n3 = faceNormal; + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = iNext; + this.faces.Add(newFace1); - newViewerFace.uv1 = newLayer.faceUVs[face.v1 - coordsLen]; - newViewerFace.uv2 = newLayer.faceUVs[face.v2 - coordsLen]; - newViewerFace.uv3 = newLayer.faceUVs[face.v3 - coordsLen]; + newFace2.v1 = iNext; + newFace2.v2 = i - numVerts; + newFace2.v3 = iNext - numVerts; + this.faces.Add(newFace2); - if (pathType == PathType.Linear) - { - newViewerFace.uv1.Flip(); - newViewerFace.uv2.Flip(); - newViewerFace.uv3.Flip(); } - - this.viewerFaces.Add(newViewerFace); } } + lastV = thisV; } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) @@ -2138,51 +1610,17 @@ namespace PrimMesher copy.radius = this.radius; copy.revolutions = this.revolutions; copy.stepsPerRevolution = this.stepsPerRevolution; - copy.calcVertexNormals = this.calcVertexNormals; - copy.normalsProcessed = this.normalsProcessed; - copy.viewerMode = this.viewerMode; + copy.numPrimFaces = this.numPrimFaces; copy.errorMessage = this.errorMessage; copy.coords = new List(this.coords); copy.faces = new List(this.faces); - copy.viewerFaces = new List(this.viewerFaces); - copy.normals = new List(this.normals); return copy; } /// - /// Calculate surface normals for all of the faces in the list of faces in this mesh - /// - public void CalcNormals() - { - if (normalsProcessed) - return; - - normalsProcessed = true; - - int numFaces = faces.Count; - - if (!this.calcVertexNormals) - this.normals = new List(); - - for (int i = 0; i < numFaces; i++) - { - Face face = faces[i]; - - this.normals.Add(SurfaceNormal(i).Normalize()); - - int normIndex = normals.Count - 1; - face.n1 = normIndex; - face.n2 = normIndex; - face.n3 = normIndex; - - this.faces[i] = face; - } - } - - /// /// Adds a value to each XYZ vertex coordinate in the mesh /// /// @@ -2202,18 +1640,6 @@ namespace PrimMesher vert.Z += z; this.coords[i] = vert; } - - if (this.viewerFaces != null) - { - int numViewerFaces = this.viewerFaces.Count; - - for (i = 0; i < numViewerFaces; i++) - { - ViewerFace v = this.viewerFaces[i]; - v.AddPos(x, y, z); - this.viewerFaces[i] = v; - } - } } /// @@ -2227,38 +1653,11 @@ namespace PrimMesher for (i = 0; i < numVerts; i++) this.coords[i] *= q; - - if (this.normals != null) - { - int numNormals = this.normals.Count; - for (i = 0; i < numNormals; i++) - this.normals[i] *= q; - } - - if (this.viewerFaces != null) - { - int numViewerFaces = this.viewerFaces.Count; - - for (i = 0; i < numViewerFaces; i++) - { - ViewerFace v = this.viewerFaces[i]; - v.v1 *= q; - v.v2 *= q; - v.v3 *= q; - - v.n1 *= q; - v.n2 *= q; - v.n3 *= q; - this.viewerFaces[i] = v; - } - } } #if VERTEX_INDEXER public VertexIndexer GetVertexIndexer() { - if (this.viewerMode && this.viewerFaces.Count > 0) - return new VertexIndexer(this); return null; } #endif @@ -2278,21 +1677,6 @@ namespace PrimMesher Coord m = new Coord(x, y, z); for (i = 0; i < numVerts; i++) this.coords[i] *= m; - - if (this.viewerFaces != null) - { - int numViewerFaces = this.viewerFaces.Count; - for (i = 0; i < numViewerFaces; i++) - { - ViewerFace v = this.viewerFaces[i]; - v.v1 *= m; - v.v2 *= m; - v.v3 *= m; - this.viewerFaces[i] = v; - } - - } - } /// diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 0df71eb..5030cec 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -448,7 +448,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { repData.meshState = MeshState.needMesh; - mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, false, convex, true); + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); if (mesh == null) { repData.meshState = MeshState.MeshFailed; @@ -513,7 +513,7 @@ namespace OpenSim.Region.Physics.OdePlugin clod = (int)LevelOfDetail.Low; } - mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, false, convex, true); + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); if (mesh == null) { @@ -929,4 +929,4 @@ namespace OpenSim.Region.Physics.OdePlugin repData.actor.Name); } } -} +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index faa9488..7cabddd 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -25,7 +25,7 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -/* Revision 2011/12 by Ubit Umarov +/* Revision 2011/12/13 by Ubit Umarov * * */ @@ -1736,7 +1736,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - d.BodySetDamping(Body, .005f, .005f); + d.BodySetAutoDisableAngularThreshold(Body, 0.01f); + d.BodySetAutoDisableLinearThreshold(Body, 0.01f); + d.BodySetDamping(Body, .005f, .001f); if (m_targetSpace != IntPtr.Zero) { @@ -2144,7 +2146,7 @@ namespace OpenSim.Region.Physics.OdePlugin _mass = primMass; // just in case - d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z); + d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z); d.MassTranslate(ref primdMass, m_OBBOffset.X, @@ -2362,6 +2364,7 @@ namespace OpenSim.Region.Physics.OdePlugin MakeBody(); } + #region changes private void changeadd() @@ -3213,7 +3216,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (++bodydisablecontrol < 20) return; - d.BodyEnable(Body); } @@ -3381,11 +3383,12 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public void UpdatePositionAndVelocity() + public void UpdatePositionAndVelocity(int frame) { if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) { - if (d.BodyIsEnabled(Body) || !_zeroFlag) + bool bodyenabled = d.BodyIsEnabled(Body); + if (bodyenabled || !_zeroFlag) { bool lastZeroFlag = _zeroFlag; @@ -3478,13 +3481,13 @@ namespace OpenSim.Region.Physics.OdePlugin // tolerance values depende a lot on simulation noise... // use simple math.abs since we dont need to be exact - if ( - (Math.Abs(_position.X - lpos.X) < 0.001f) - && (Math.Abs(_position.Y - lpos.Y) < 0.001f) - && (Math.Abs(_position.Z - lpos.Z) < 0.001f) - && (Math.Abs(_orientation.X - ori.X) < 0.0001f) - && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f) - && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W + if (!bodyenabled || + (Math.Abs(_position.X - lpos.X) < 0.005f) + && (Math.Abs(_position.Y - lpos.Y) < 0.005f) + && (Math.Abs(_position.Z - lpos.Z) < 0.005f) + && (Math.Abs(_orientation.X - ori.X) < 0.001f) + && (Math.Abs(_orientation.Y - ori.Y) < 0.001f) + && (Math.Abs(_orientation.Z - ori.Z) < 0.001f) // ignore W ) { _zeroFlag = true; @@ -3499,9 +3502,9 @@ namespace OpenSim.Region.Physics.OdePlugin _acceleration = _velocity; - if ((Math.Abs(vel.X) < 0.001f) && - (Math.Abs(vel.Y) < 0.001f) && - (Math.Abs(vel.Z) < 0.001f)) + if ((Math.Abs(vel.X) < 0.005f) && + (Math.Abs(vel.Y) < 0.005f) && + (Math.Abs(vel.Z) < 0.005f)) { _velocity = Vector3.Zero; float t = -m_invTimeStep; @@ -3538,6 +3541,15 @@ namespace OpenSim.Region.Physics.OdePlugin } } + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + if (_zeroFlag) { if (lastZeroFlag) @@ -3556,14 +3568,6 @@ namespace OpenSim.Region.Physics.OdePlugin return; } - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; base.RequestPhysicsterseUpdate(); m_lastUpdateSent = false; } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs index 2923ccf..73ababa 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -680,4 +680,4 @@ namespace OpenSim.Region.Physics.OdePlugin public RayFilterFlags filter; public Quaternion orientation; } -} +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 10d7d50..1f09dc7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -1869,7 +1869,7 @@ namespace OdeAPI [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] public static extern void SpaceSetCleanup(IntPtr space, bool mode); - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] + [DllImport("ode.dll", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] public static extern void SpaceSetSublevel(IntPtr space, int sublevel); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity] diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 754bc86..49dc03c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -25,6 +25,7 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +// Revision 2011/12/13 by Ubit Umarov //#define SPAM using System; @@ -43,25 +44,7 @@ using OpenMetaverse; namespace OpenSim.Region.Physics.OdePlugin { - public enum StatusIndicators : int - { - Generic = 0, - Start = 1, - End = 2 - } - - public struct sCollisionData - { - public uint ColliderLocalId; - public uint CollidedWithLocalId; - public int NumberOfCollisions; - public int CollisionType; - public int StatusIndicator; - public int lastframe; - } - - - // colision flags of things others can colide with + // colision flags of things others can colide with // rays, sensors, probes removed since can't be colided with // The top space where things are placed provided further selection // ie physical are in active space nonphysical in static @@ -188,12 +171,14 @@ namespace OpenSim.Region.Physics.OdePlugin public bool OdeUbitLib = false; // private int threadid = 0; - private Random fluidRandomizer = new Random(Environment.TickCount); +// private Random fluidRandomizer = new Random(Environment.TickCount); + +// const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; - const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; - const float MaxERP = 0.8f; - const float minERP = 0.1f; + const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1; + const float comumContactERP = 0.7f; const float comumContactCFM = 0.0001f; + const float comumContactSLIP = 0.000001f; float frictionMovementMult = 0.8f; @@ -236,8 +221,8 @@ namespace OpenSim.Region.Physics.OdePlugin public float geomDefaultDensity = 10.000006836f; - public int geomContactPointsStartthrottle = 3; - public int geomUpdatesPerThrottledUpdate = 15; +// public int geomContactPointsStartthrottle = 3; +// public int geomUpdatesPerThrottledUpdate = 15; public float bodyPIDD = 35f; public float bodyPIDG = 25; @@ -246,7 +231,6 @@ namespace OpenSim.Region.Physics.OdePlugin public int bodyFramesAutoDisable = 5; - private d.NearCallback nearCallback; private HashSet _characters = new HashSet(); @@ -266,11 +250,12 @@ namespace OpenSim.Region.Physics.OdePlugin // public Dictionary geom_name_map = new Dictionary(); public Dictionary actor_name_map = new Dictionary(); - private float contactsurfacelayer = 0.002f; + private float contactsurfacelayer = 0.001f; private int contactsPerCollision = 80; internal IntPtr ContactgeomsArray = IntPtr.Zero; private IntPtr GlobalContactsArray = IntPtr.Zero; + private d.Contact SharedTmpcontact = new d.Contact(); const int maxContactsbeforedeath = 4000; private volatile int m_global_contactcount = 0; @@ -283,7 +268,7 @@ namespace OpenSim.Region.Physics.OdePlugin private Dictionary TerrainHeightFieldHeights = new Dictionary(); private Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); - private int m_physicsiterations = 10; + private int m_physicsiterations = 15; private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag // private PhysicsActor PANull = new NullPhysicsActor(); private float step_time = 0.0f; @@ -303,8 +288,6 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr StaticSpace; // space for the static things around public IntPtr GroundSpace; // space for ground - public IntPtr SharedRay; - // some speedup variables private int spaceGridMaxX; private int spaceGridMaxY; @@ -431,8 +414,6 @@ namespace OpenSim.Region.Physics.OdePlugin contactgroup = d.JointGroupCreate(0); //contactgroup - SharedRay = d.CreateRay(TopSpace, 1.0f); - d.WorldSetAutoDisableFlag(world, false); } } @@ -481,10 +462,10 @@ namespace OpenSim.Region.Physics.OdePlugin metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); - contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); +// contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); - m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations); +// m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations); avDensity = physicsconfig.GetFloat("av_density", avDensity); avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); @@ -492,8 +473,8 @@ namespace OpenSim.Region.Physics.OdePlugin contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); - geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); - geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); +// geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); +// geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); // geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); @@ -508,6 +489,23 @@ namespace OpenSim.Region.Physics.OdePlugin } } + + d.WorldSetCFM(world, comumContactCFM); + d.WorldSetERP(world, comumContactERP); + + d.WorldSetGravity(world, gravityx, gravityy, gravityz); + + d.WorldSetLinearDamping(world, 0.002f); + d.WorldSetAngularDamping(world, 0.002f); + d.WorldSetAngularDampingThreshold(world, 0f); + d.WorldSetLinearDampingThreshold(world, 0f); + d.WorldSetMaxAngularSpeed(world, 100f); + + d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + + d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); + d.WorldSetContactMaxCorrectingVel(world, 60.0f); + m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); HalfOdeStep = ODE_STEPSIZE * 0.5f; @@ -516,6 +514,20 @@ namespace OpenSim.Region.Physics.OdePlugin ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); + SharedTmpcontact.geom.g1 = IntPtr.Zero; + SharedTmpcontact.geom.g2 = IntPtr.Zero; + + SharedTmpcontact.geom.side1 = -1; + SharedTmpcontact.geom.side2 = -1; + + SharedTmpcontact.surface.mode = comumContactFlags; + SharedTmpcontact.surface.mu = 0; + SharedTmpcontact.surface.bounce = 0; + SharedTmpcontact.surface.soft_cfm = comumContactCFM; + SharedTmpcontact.surface.soft_erp = comumContactERP; + SharedTmpcontact.surface.slip1 = comumContactSLIP; + SharedTmpcontact.surface.slip2 = comumContactSLIP; + m_materialContactsData[(int)Material.Stone].mu = 0.8f; m_materialContactsData[(int)Material.Stone].bounce = 0.4f; @@ -540,27 +552,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_materialContactsData[(int)Material.light].mu = 0.0f; m_materialContactsData[(int)Material.light].bounce = 0.0f; - // Set the gravity,, don't disable things automatically (we set it explicitly on some things) - - d.WorldSetGravity(world, gravityx, gravityy, gravityz); - d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); - - d.WorldSetLinearDamping(world, 0.002f); - d.WorldSetAngularDamping(world, 0.002f); - d.WorldSetAngularDampingThreshold(world, 0f); - d.WorldSetLinearDampingThreshold(world, 0f); - d.WorldSetMaxAngularSpeed(world, 100f); - - d.WorldSetCFM(world,1e-6f); // a bit harder than default - //d.WorldSetCFM(world, 1e-4f); // a bit harder than default - d.WorldSetERP(world, 0.6f); // higher than original - - // Set how many steps we go without running collision testing - // This is in addition to the step size. - // Essentially Steps * m_physicsiterations - d.WorldSetQuickStepNumIterations(world, m_physicsiterations); - - d.WorldSetContactMaxCorrectingVel(world, 60.0f); spacesPerMeter = 1 / metersInSpace; spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); @@ -631,40 +622,21 @@ namespace OpenSim.Region.Physics.OdePlugin // sets a global contact for a joint for contactgeom , and base contact description) - private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale) + + + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom) { - if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) + if (m_global_contactcount >= maxContactsbeforedeath) return IntPtr.Zero; - float erp = contactGeom.depth; - erp *= erpscale; - if (erp < minERP) - erp = minERP; - else if (erp > MaxERP) - erp = MaxERP; - - float depth = contactGeom.depth * dscale; - if (depth > 0.5f) - depth = 0.5f; - - d.Contact newcontact = new d.Contact(); - newcontact.geom.depth = depth; - newcontact.geom.g1 = contactGeom.g1; - newcontact.geom.g2 = contactGeom.g2; - newcontact.geom.pos = contactGeom.pos; - newcontact.geom.normal = contactGeom.normal; - newcontact.geom.side1 = contactGeom.side1; - newcontact.geom.side2 = contactGeom.side2; - - // this needs bounce also - newcontact.surface.mode = comumContactFlags; - newcontact.surface.mu = mu; - newcontact.surface.bounce = bounce; - newcontact.surface.soft_cfm = cfm; - newcontact.surface.soft_erp = erp; + m_global_contactcount++; + + SharedTmpcontact.geom.depth = contactGeom.depth; + SharedTmpcontact.geom.pos = contactGeom.pos; + SharedTmpcontact.geom.normal = contactGeom.normal; IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); - Marshal.StructureToPtr(newcontact, contact, true); + Marshal.StructureToPtr(SharedTmpcontact, contact, true); return d.JointCreateContactPtr(world, contactgroup, contact); } @@ -825,10 +797,12 @@ namespace OpenSim.Region.Physics.OdePlugin if (!GetCurContactGeom(0, ref curContact)) return; + ContactPoint maxDepthContact = new ContactPoint(); + // do volume detection case if ((p1.IsVolumeDtc || p2.IsVolumeDtc)) { - ContactPoint maxDepthContact = new ContactPoint( + maxDepthContact = new ContactPoint( new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), curContact.depth, false @@ -842,10 +816,7 @@ namespace OpenSim.Region.Physics.OdePlugin float mu = 0; float bounce = 0; - float cfm = 0.0001f; - float erpscale = 1.0f; - float dscale = 1.0f; - bool IgnoreNegSides = false; +// bool IgnoreNegSides = false; ContactData contactdata1 = new ContactData(0, 0, false); ContactData contactdata2 = new ContactData(0, 0, false); @@ -890,7 +861,9 @@ namespace OpenSim.Region.Physics.OdePlugin break; case (int)ActorTypes.Prim: - if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) + Vector3 relV = p1.Velocity - p2.Velocity; + float relVlenSQ = relV.LengthSquared(); + if (relVlenSQ > 0.0001f) { p1.CollidingObj = true; p2.CollidingObj = true; @@ -900,21 +873,7 @@ namespace OpenSim.Region.Physics.OdePlugin bounce = contactdata1.bounce * contactdata2.bounce; mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - cfm = p1.Mass; - if (cfm > p2.Mass) - cfm = p2.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - if (dscale > 1.0f) - dscale = 1.0f; - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; - else if (cfm < 0.00001f) - cfm = 0.00001f; - - if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + if (relVlenSQ > 0.01f) mu *= frictionMovementMult; break; @@ -923,27 +882,17 @@ namespace OpenSim.Region.Physics.OdePlugin p1.getContactData(ref contactdata1); bounce = contactdata1.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) mu *= frictionMovementMult; p1.CollidingGround = true; - - cfm = p1.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - if (dscale > 1.0f) - dscale = 1.0f; - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; - else if (cfm < 0.00001f) - cfm = 0.00001f; - +/* if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) { if (curContact.side1 > 0) IgnoreNegSides = true; } + */ break; case (int)ActorTypes.Water: @@ -961,22 +910,8 @@ namespace OpenSim.Region.Physics.OdePlugin bounce = contactdata2.bounce * TerrainBounce; mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); - cfm = p2.Mass; - dscale = 10 / cfm; - dscale = (float)Math.Sqrt(dscale); - - if (dscale > 1.0f) - dscale = 1.0f; - - erpscale = cfm * 0.01f; - cfm = 0.0001f / cfm; - if (cfm > 0.01f) - cfm = 0.01f; - else if (cfm < 0.00001f) - cfm = 0.00001f; - - if (curContact.side1 > 0) // should be 2 ? - IgnoreNegSides = true; +// if (curContact.side1 > 0) // should be 2 ? +// IgnoreNegSides = true; if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) mu *= frictionMovementMult; @@ -993,26 +928,24 @@ namespace OpenSim.Region.Physics.OdePlugin if (ignore) return; - - d.ContactGeom maxContact = curContact; - // if (IgnoreNegSides && curContact.side1 < 0) - // maxContact.depth = float.MinValue; - - d.ContactGeom minContact = curContact; - // if (IgnoreNegSides && curContact.side1 < 0) - // minContact.depth = float.MaxValue; - IntPtr Joint; bool FeetCollision = false; int ncontacts = 0; - int i = 0; + maxDepthContact = new ContactPoint(); + maxDepthContact.PenetrationDepth = float.MinValue; + ContactPoint minDepthContact = new ContactPoint(); + minDepthContact.PenetrationDepth = float.MaxValue; + + SharedTmpcontact.geom.depth = 0; + SharedTmpcontact.surface.mu = mu; + SharedTmpcontact.surface.bounce = bounce; + while (true) { - -// if (!(IgnoreNegSides && curContact.side1 < 0)) +// if (!(IgnoreNegSides && curContact.side1 < 0)) { bool noskip = true; if (dop1ava) @@ -1029,26 +962,32 @@ namespace OpenSim.Region.Physics.OdePlugin if (noskip) { - m_global_contactcount++; - if (m_global_contactcount >= maxContactsbeforedeath) - break; - - ncontacts++; - - Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); + Joint = CreateContacJoint(ref curContact); if (Joint == IntPtr.Zero) break; d.JointAttach(Joint, b1, b2); - if (curContact.depth > maxContact.depth) - maxContact = curContact; + ncontacts++; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.PenetrationDepth = curContact.depth; + maxDepthContact.CharacterFeet = FeetCollision; + } - if (curContact.depth < minContact.depth) - minContact = curContact; + if (curContact.depth < minDepthContact.PenetrationDepth) + { + minDepthContact.PenetrationDepth = curContact.depth; + minDepthContact.SurfaceNormal.X = curContact.normal.X; + minDepthContact.SurfaceNormal.Y = curContact.normal.Y; + minDepthContact.SurfaceNormal.Z = curContact.normal.Z; + } } } - if (++i >= count) break; @@ -1058,11 +997,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (ncontacts > 0) { - ContactPoint maxDepthContact = new ContactPoint( - new Vector3(maxContact.pos.X, maxContact.pos.Y, maxContact.pos.Z), - new Vector3(minContact.normal.X, minContact.normal.Y, minContact.normal.Z), - maxContact.depth, FeetCollision - ); + maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X; + maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y; + maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z; + collision_accounting_events(p1, p2, maxDepthContact); } } @@ -1629,16 +1567,15 @@ namespace OpenSim.Region.Physics.OdePlugin if (framecount < 0) framecount = 0; - framecount++; - int curphysiteractions; +// int curphysiteractions; // if in trouble reduce step resolution - if (step_time >= m_SkipFramesAtms) - curphysiteractions = m_physicsiterations / 2; - else - curphysiteractions = m_physicsiterations; +// if (step_time >= m_SkipFramesAtms) +// curphysiteractions = m_physicsiterations / 2; +// else +// curphysiteractions = m_physicsiterations; // checkThread(); int nodeframes = 0; @@ -1653,14 +1590,12 @@ namespace OpenSim.Region.Physics.OdePlugin } ODEchangeitem item; - - - d.WorldSetQuickStepNumIterations(world, curphysiteractions); +// d.WorldSetQuickStepNumIterations(world, curphysiteractions); int loopstartMS = Util.EnvironmentTickCount(); int looptimeMS = 0; - + while (step_time > HalfOdeStep) { @@ -1763,6 +1698,7 @@ namespace OpenSim.Region.Physics.OdePlugin // do a ode simulation step d.WorldQuickStep(world, ODE_STEPSIZE); +// d.WorldStep(world, ODE_STEPSIZE); d.JointGroupEmpty(contactgroup); // update managed ideia of physical data and do updates to core @@ -1789,7 +1725,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (actor.IsPhysical) { - actor.UpdatePositionAndVelocity(); + actor.UpdatePositionAndVelocity(framecount); } } } -- cgit v1.1 From f00ecf5fe71df4eb5ad5dd1c195616300e988820 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 May 2013 02:18:05 +0100 Subject: missing files --- OpenSim/Region/Physics/Manager/IMesher.cs | 1 + OpenSim/Region/Physics/Manager/ZeroMesher.cs | 10 +++++----- 2 files changed, 6 insertions(+), 5 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index 5485eb7..e290dc9 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -37,6 +37,7 @@ namespace OpenSim.Region.Physics.Manager { IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod); IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical); + IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde); IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde); IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex); void ReleaseMesh(IMesh mesh); diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs index 80ecf66..890951f 100644 --- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs +++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs @@ -64,20 +64,20 @@ namespace OpenSim.Region.Physics.Manager { public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) { - return CreateMesh(primName, primShape, size, lod, false, false); + return CreateMesh(primName, primShape, size, lod, false); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex,bool forOde) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde) { return CreateMesh(primName, primShape, size, lod, false); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex,bool forOde) { - return CreateMesh(primName, primShape, size, lod, false, false); + return CreateMesh(primName, primShape, size, lod, false); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { // Remove the reference to the encoded JPEG2000 data so it can be GCed primShape.SculptData = OpenMetaverse.Utils.EmptyBytes; -- cgit v1.1 From 088284677019b959c2d140eedbd4e5d3baabe380 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 May 2013 02:21:02 +0100 Subject: missing file --- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index d181b78..a32f401 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -715,6 +715,11 @@ namespace OpenSim.Region.Physics.Meshing return CreateMesh(primName, primShape, size, lod, isPhysical, true); } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) + { + return CreateMesh(primName, primShape, size, lod, isPhysical, true); + } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache) { #if SPAM -- cgit v1.1 From 1cf24b70925f206f334fadb0baf20779aa9793f6 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 May 2013 04:47:59 +0100 Subject: make sure friction slip parameters are zero ( or other value ) and not default cmf --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 49dc03c..8abf6cf 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -175,10 +175,10 @@ namespace OpenSim.Region.Physics.OdePlugin // const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; - const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1; + const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2; const float comumContactERP = 0.7f; const float comumContactCFM = 0.0001f; - const float comumContactSLIP = 0.000001f; + const float comumContactSLIP = 0f; float frictionMovementMult = 0.8f; @@ -411,7 +411,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); d.GeomSetCollideBits(GroundSpace, 0); - contactgroup = d.JointGroupCreate(0); + contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1); //contactgroup d.WorldSetAutoDisableFlag(world, false); -- cgit v1.1 From 269febc87e1aba24fff09e9fb0d77eee0a73b185 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 May 2013 06:32:26 +0100 Subject: let gravity modifier, friction, restitution and density changes be applied to prim. Only have efect on root prim. Density doesn't get effect imediatly, only on next change of size or shape. density change implies a full body rebuild to be done later, after reducing the number of sets sop does. Other parameters should work. **** mainly untested *** --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 19 +++++--- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 52 +++++++++++++++++++++- 2 files changed, 65 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index a7dda7a..3c952ae 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -137,6 +137,7 @@ namespace OpenSim.Region.Physics.OdePlugin float m_amdampY; float m_amdampZ; + float m_gravmod; public float FrictionFactor { @@ -146,6 +147,14 @@ namespace OpenSim.Region.Physics.OdePlugin } } + public float GravMod + { + set + { + m_gravmod = value; + } + } + public ODEDynamics(OdePrim rootp) { @@ -153,6 +162,7 @@ namespace OpenSim.Region.Physics.OdePlugin _pParentScene = rootPrim._parent_scene; m_timestep = _pParentScene.ODE_STEPSIZE; m_invtimestep = 1.0f / m_timestep; + m_gravmod = rootPrim.GravModifier; } public void DoSetVehicle(VehicleData vd) @@ -816,7 +826,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_lmEfect = 0; m_ffactor = 1f; } - + // hover if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) { @@ -862,7 +872,7 @@ namespace OpenSim.Region.Physics.OdePlugin force.Z += perr; ldampZ *= -curVel.Z; - force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); } else // no buoyancy force.Z += _pParentScene.gravityz; @@ -870,7 +880,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { // default gravity and Buoyancy - force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); } // linear deflection @@ -1063,8 +1073,7 @@ namespace OpenSim.Region.Physics.OdePlugin torque.Y -= curLocalAngVel.Y * m_amdampY; torque.Z -= curLocalAngVel.Z * m_amdampZ; } - - + if (force.X != 0 || force.Y != 0 || force.Z != 0) { diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 7cabddd..e3f88c6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -115,6 +115,7 @@ namespace OpenSim.Region.Physics.OdePlugin private int body_autodisable_frames; public int bodydisablecontrol; + private float m_gravmod = 1.0f; // Default we're a Geometry @@ -914,6 +915,55 @@ namespace OpenSim.Region.Physics.OdePlugin bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; } + public override float Density + { + get + { + return m_density * 100f; + } + set + { + m_density = value / 100f; + // for not prim mass is not updated since this implies full rebuild of body inertia TODO + } + } + public override float GravModifier + { + get + { + return m_gravmod; + } + set + { + m_gravmod = value; + if (m_vehicle != null) + m_vehicle.GravMod = m_gravmod; + } + } + public override float Friction + { + get + { + return mu; + } + set + { + mu = value; + } + } + + public override float Restitution + { + get + { + return bounce; + } + set + { + bounce = value; + } + } + public void setPrimForRemoval() { AddChange(changes.Remove, null); @@ -3336,7 +3386,7 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - float b = (1.0f - m_buoyancy); + float b = (1.0f - m_buoyancy) * m_gravmod; fx = _parent_scene.gravityx * b; fy = _parent_scene.gravityy * b; fz = _parent_scene.gravityz * b; -- cgit v1.1 From d5b1baffece161a21248f9fa84164cfcdd2fd813 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 May 2013 06:45:25 +0100 Subject: correction to previus comment: all parameters but gravity modifier have efect on all prims. --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 1 - 1 file changed, 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index e3f88c6..c247e68 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -117,7 +117,6 @@ namespace OpenSim.Region.Physics.OdePlugin public int bodydisablecontrol; private float m_gravmod = 1.0f; - // Default we're a Geometry private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); // Default colide nonphysical don't try to colide with anything -- cgit v1.1 From a6b964aa11eb57011b4d85ce16f22c5ac663b1ed Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 May 2013 19:51:45 +0100 Subject: removed spurius ref to ode.dll from odeapi --- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 1f09dc7..10d7d50 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -1869,7 +1869,7 @@ namespace OdeAPI [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] public static extern void SpaceSetCleanup(IntPtr space, bool mode); - [DllImport("ode.dll", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] public static extern void SpaceSetSublevel(IntPtr space, int sublevel); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity] -- cgit v1.1 From 4709436a6ca5daecad3f0835fd6c4921df7a5339 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 May 2013 20:23:52 +0100 Subject: retouch prim stopped condition test --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index c247e68..4cac0aa 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -3534,9 +3534,9 @@ namespace OpenSim.Region.Physics.OdePlugin (Math.Abs(_position.X - lpos.X) < 0.005f) && (Math.Abs(_position.Y - lpos.Y) < 0.005f) && (Math.Abs(_position.Z - lpos.Z) < 0.005f) - && (Math.Abs(_orientation.X - ori.X) < 0.001f) - && (Math.Abs(_orientation.Y - ori.Y) < 0.001f) - && (Math.Abs(_orientation.Z - ori.Z) < 0.001f) // ignore W + && (Math.Abs(_orientation.X - ori.X) < 0.0005f) + && (Math.Abs(_orientation.Y - ori.Y) < 0.0005f) + && (Math.Abs(_orientation.Z - ori.Z) < 0.0005f) // ignore W ) { _zeroFlag = true; -- cgit v1.1 From 748fc66ef10591fb0cf3c82d6838561bf48ac197 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 18 Jul 2014 13:19:24 +0100 Subject: reduce avatar hover in basic ode plugin --- OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs index 319f6ab..86735de 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -500,7 +500,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_pidControllerActive = true; - m_tainted_CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; + m_tainted_CAPSULE_LENGTH = (size.Z) - CAPSULE_RADIUS * 2.0f; // m_log.Info("[ODE CHARACTER]: " + CAPSULE_LENGTH); } else -- cgit v1.1 From 9654b81b2d73e524ad5541bc38440949f4952bad Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 19 Jul 2014 16:16:13 +0100 Subject: revert to capsule representation of avatar collider --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 325 ++------------------- 1 file changed, 28 insertions(+), 297 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index e912997..ecd5474 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -95,10 +95,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_feetOffset = 0; private float feetOff = 0; - private float feetSZ = 0.5f; - const float feetScale = 0.8f; private float boneOff = 0; - private float m_lastVelocitySqr = 0; public float walkDivisor = 1.3f; public float runDivisor = 0.8f; @@ -110,7 +107,6 @@ namespace OpenSim.Region.Physics.OdePlugin private bool _zeroFlag = false; - private int m_requestedUpdateFrequency = 0; private uint m_localID = 0; public bool m_returnCollisions = false; // taints and their non-tainted counterparts @@ -127,7 +123,6 @@ namespace OpenSim.Region.Physics.OdePlugin int m_colliderfilter = 0; int m_colliderGroundfilter = 0; int m_colliderObjectfilter = 0; - bool m_collisionException = false; // Default we're a Character private CollisionCategories m_collisionCategories = (CollisionCategories.Character); @@ -140,9 +135,7 @@ namespace OpenSim.Region.Physics.OdePlugin // we do land collisions not ode | CollisionCategories.Land); public IntPtr Body = IntPtr.Zero; private OdeScene _parent_scene; - private IntPtr topbox = IntPtr.Zero; - private IntPtr midbox = IntPtr.Zero; - private IntPtr feetbox = IntPtr.Zero; + private IntPtr capsule = IntPtr.Zero; private IntPtr bbox = IntPtr.Zero; public IntPtr collider = IntPtr.Zero; @@ -150,9 +143,6 @@ namespace OpenSim.Region.Physics.OdePlugin public d.Mass ShellMass; - - - public int m_eventsubscription = 0; private int m_cureventsubscription = 0; private CollisionEventUpdate CollisionEventsThisFrame = null; @@ -214,8 +204,6 @@ namespace OpenSim.Region.Physics.OdePlugin // force lower density for testing m_density = 3.0f; - m_density *= 1.4f; // scale to have mass similar to capsule - mu = parent_scene.AvatarFriction; walkDivisor = walk_divisor; @@ -704,58 +692,6 @@ namespace OpenSim.Region.Physics.OdePlugin AddChange(changes.Momentum, momentum); } - private void ajustCollider() - { - float vq = _velocity.LengthSquared(); - if (m_lastVelocitySqr != vq) - { - m_lastVelocitySqr = vq; - if (vq > 100.0f) - { - Vector3 off = _velocity; - float t = 0.5f * timeStep; - off = off * t; - d.Quaternion qtmp; - d.GeomCopyQuaternion(bbox, out qtmp); - Quaternion q; - q.X = qtmp.X; - q.Y = qtmp.Y; - q.Z = qtmp.Z; - q.W = qtmp.W; - off *= Quaternion.Conjugate(q); - - d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z); - - off.X = 2.0f * (m_size.X + Math.Abs(off.X)); - off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y)); - off.Z = m_size.Z + 2.0f * Math.Abs(off.Z); - d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z); - - d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(bbox, (uint)m_collisionFlags); - d.GeomSetCategoryBits(topbox, 0); - d.GeomSetCollideBits(topbox, 0); - d.GeomSetCategoryBits(midbox, 0); - d.GeomSetCollideBits(midbox, 0); - d.GeomSetCategoryBits(feetbox, 0); - d.GeomSetCollideBits(feetbox, 0); - } - else - { - d.GeomSetCategoryBits(bbox, 0); - d.GeomSetCollideBits(bbox, 0); - d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); - d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); - d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); - d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); - } - uint cat1 = d.GeomGetCategoryBits(bbox); - uint col1 = d.GeomGetCollideBits(bbox); - - } - } private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) { @@ -764,37 +700,14 @@ namespace OpenSim.Region.Physics.OdePlugin float sy = m_size.Y; float sz = m_size.Z; - float topsx = sx * 0.9f; - float midsx = sx; - float feetsx = sx * feetScale; - float bonesx = sx * 0.2f; - - float topsy = sy * 0.4f; - float midsy = sy; - float feetsy = sy * feetScale * 0.8f; - float bonesy = feetsy * 0.2f; + float bot = -sz * 0.5f + m_feetOffset; + boneOff = bot + 0.3f; - float topsz = sz * 0.15f; float feetsz = sz * 0.45f; if (feetsz > 0.6f) feetsz = 0.6f; - float midsz = sz - topsz - feetsz; - float bonesz = sz; - - float bot = -sz * 0.5f + m_feetOffset; - - boneOff = bot + 0.3f; - - float feetz = bot + feetsz * 0.5f; - bot += feetsz; - - feetOff = bot; - feetSZ = feetsz; - - float midz = bot + midsz * 0.5f; - bot += midsz; - float topz = bot + topsz * 0.5f; + feetOff = bot + feetsz; _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); @@ -805,9 +718,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); d.GeomSetCollideBits(collider, (uint)m_collisionFlags); - feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz); - midbox = d.CreateBox(collider, midsx, midsy, midsz); - topbox = d.CreateBox(collider, topsx, topsy, topsz); + float r = m_size.X; + if (m_size.Y > r) + r = m_size.Y; + float l = m_size.Z - r; + r *= 0.5f; + capsule = d.CreateCapsule(collider, r, l); + bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z); m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass @@ -820,12 +737,10 @@ namespace OpenSim.Region.Physics.OdePlugin Body = d.BodyCreate(_parent_scene.world); _zeroFlag = false; - m_collisionException = false; m_pidControllerActive = true; m_freemove = false; _velocity = Vector3.Zero; - m_lastVelocitySqr = 0; d.BodySetAutoDisableFlag(Body, false); d.BodySetPosition(Body, npositionX, npositionY, npositionZ); @@ -835,17 +750,9 @@ namespace OpenSim.Region.Physics.OdePlugin _position.Z = npositionZ; d.BodySetMass(Body, ref ShellMass); - d.GeomSetBody(feetbox, Body); - d.GeomSetBody(midbox, Body); - d.GeomSetBody(topbox, Body); - d.GeomSetBody(bbox, Body); - - d.GeomSetOffsetPosition(feetbox, 0, 0, feetz); - d.GeomSetOffsetPosition(midbox, 0, 0, midz); - d.GeomSetOffsetPosition(topbox, 0, 0, topz); - - ajustCollider(); + d.GeomSetBody(bbox, Body); + d.GeomSetBody(capsule, Body); // The purpose of the AMotor here is to keep the avatar's physical // surrogate from rotating while moving @@ -906,26 +813,12 @@ namespace OpenSim.Region.Physics.OdePlugin } //kill the Geoms - if (topbox != IntPtr.Zero) + if (capsule != IntPtr.Zero) { - _parent_scene.actor_name_map.Remove(topbox); + _parent_scene.actor_name_map.Remove(capsule); _parent_scene.waitForSpaceUnlock(collider); - d.GeomDestroy(topbox); - topbox = IntPtr.Zero; - } - if (midbox != IntPtr.Zero) - { - _parent_scene.actor_name_map.Remove(midbox); - _parent_scene.waitForSpaceUnlock(collider); - d.GeomDestroy(midbox); - midbox = IntPtr.Zero; - } - if (feetbox != IntPtr.Zero) - { - _parent_scene.actor_name_map.Remove(feetbox); - _parent_scene.waitForSpaceUnlock(collider); - d.GeomDestroy(feetbox); - feetbox = IntPtr.Zero; + d.GeomDestroy(capsule); + capsule = IntPtr.Zero; } if (bbox != IntPtr.Zero) @@ -981,163 +874,21 @@ namespace OpenSim.Region.Physics.OdePlugin public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) { feetcollision = false; - if (m_collisionException) - return false; Vector3 offset; - if (me == bbox) // if moving fast - { - // force a full inelastic collision - m_collisionException = true; - - offset = m_size * m_orientation2D; - - offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth; - offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth; - offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth; - - if (reverse) - { - offset.X *= -contact.normal.X; - offset.Y *= -contact.normal.Y; - offset.Z *= -contact.normal.Z; - } - else - { - offset.X *= contact.normal.X; - offset.Y *= contact.normal.Y; - offset.Z *= contact.normal.Z; - } - - offset.X += contact.pos.X; - offset.Y += contact.pos.Y; - offset.Z += contact.pos.Z; - - //_position = offset; - //return false; - } - offset.X = contact.pos.X - _position.X; offset.Y = contact.pos.Y - _position.Y; - if (me == topbox) - { - offset.Z = contact.pos.Z - _position.Z; - - offset.Normalize(); - - if (reverse) - { - contact.normal.X = offset.X; - contact.normal.Y = offset.Y; - contact.normal.Z = offset.Z; - } - else - { - contact.normal.X = -offset.X; - contact.normal.Y = -offset.Y; - contact.normal.Z = -offset.Z; - } - return true; - } - - if (me == midbox) - { - if (Math.Abs(contact.normal.Z) > 0.95f) - { - offset.Z = contact.pos.Z - _position.Z; - offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth; - offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth; - offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth; - - if (reverse) - { - offset.X *= -contact.normal.X; - offset.Y *= -contact.normal.Y; - offset.Z *= -contact.normal.Z; - } - else - { - offset.X *= contact.normal.X; - offset.Y *= contact.normal.Y; - offset.Z *= contact.normal.Z; - } - - offset.X += contact.pos.X; - offset.Y += contact.pos.Y; - offset.Z += contact.pos.Z; - _position = offset; - return true; - } - else - offset.Z = contact.normal.Z; - - offset.Normalize(); - - /* - if (reverse) - { - contact.normal.X = offset.X; - contact.normal.Y = offset.Y; - contact.normal.Z = offset.Z; - } - else - { - contact.normal.X = -offset.X; - contact.normal.Y = -offset.Y; - contact.normal.Z = -offset.Z; - } - */ - //_position.Z = offset.Z; - return true; - } - - else if (me == feetbox) + if (me == capsule) { float h = contact.pos.Z - _position.Z; - - // Only do this if the normal is sufficiently pointing in the 'up' direction - if (Math.Abs(contact.normal.Z) > 0.95f) - { - // We Only want to do this if we're sunk into the object a bit and we're stuck and we're trying to move and feetcollision is false - if ((contact.depth > 0.0010f && _velocity.X == 0f && _velocity.Y == 0 && _velocity.Z == 0) - && (_target_velocity.X > 0 || _target_velocity.Y > 0 || _target_velocity.Z > 0) - && (!feetcollision) ) - { - m_collisionException = true; // Stop looping, do this only once not X times Contacts - _position.Z += contact.depth + 0.01f; // Move us Up the amount that we sank in, and add 0.01 meters to gently lift avatar up. - - return true; - } - - if (contact.normal.Z > 0) - contact.normal.Z = 1.0f; - else - contact.normal.Z = -1.0f; - contact.normal.X = 0.0f; - contact.normal.Y = 0.0f; - feetcollision = true; - if (h < boneOff) - IsColliding = true; - return true; - } - - offset.Z = h - feetOff; // distance from top of feetbox + offset.Z = h - feetOff; if (offset.Z > 0) - return false; + return true; - if (offset.Z > -0.01) - { - offset.X = 0; - offset.Y = 0; - offset.Z = -1.0f; - } - else - { - offset.Normalize(); - } + offset.Normalize(); if (reverse) { @@ -1171,23 +922,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body == IntPtr.Zero) return; - if (m_collisionException) - { - d.BodySetPosition(Body,_position.X, _position.Y, _position.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - - float v = _velocity.Length(); - if (v != 0) - { - v = 5.0f / v; - _velocity = _velocity * v; - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - } - ajustCollider(); - m_collisionException = false; - return; - } - d.Vector3 dtmp = d.BodyGetPosition(Body); Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); @@ -1263,7 +997,8 @@ namespace OpenSim.Region.Physics.OdePlugin // colide with land d.AABB aabb; - d.GeomGetAABB(feetbox, out aabb); +// d.GeomGetAABB(feetbox, out aabb); + d.GeomGetAABB(capsule, out aabb); float chrminZ = aabb.MinZ; ; // move up a bit Vector3 posch = localpos; @@ -1489,7 +1224,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_rotationalVelocity.Z = dtmp.Z; Math.Round(m_rotationalVelocity.Z,3); } - ajustCollider(); } public void round(ref Vector3 v, int digits) @@ -1655,10 +1389,11 @@ namespace OpenSim.Region.Physics.OdePlugin AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); _parent_scene.actor_name_map[collider] = (PhysicsActor)this; - _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; +// _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; +// _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; +// _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; _parent_scene.AddCharacter(this); } else @@ -1714,13 +1449,10 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.actor_name_map[collider] = (PhysicsActor)this; - _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; + _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; } m_freemove = false; - m_collisionException = false; m_pidControllerActive = true; } else @@ -1851,7 +1583,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); - ajustCollider(); } private void donullchange() -- cgit v1.1 From 931434de87c7727569f3ff92cf334199bd201583 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 20 Jul 2014 09:15:29 +0100 Subject: fix physics sit for physical child prims --- OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs index e9023c3..672212f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs @@ -78,11 +78,11 @@ namespace OpenSim.Region.Physics.OdePlugin IntPtr geom = ((OdePrim)actor).prim_geom; -// Vector3 geopos = d.GeomGetPositionOMV(geom); -// Quaternion geomOri = d.GeomGetQuaternionOMV(geom); + Vector3 geopos = d.GeomGetPositionOMV(geom); + Quaternion geomOri = d.GeomGetQuaternionOMV(geom); - Vector3 geopos = actor.Position; - Quaternion geomOri = actor.Orientation; +// Vector3 geopos = actor.Position; +// Quaternion geomOri = actor.Orientation; Quaternion geomInvOri = Quaternion.Conjugate(geomOri); -- cgit v1.1 From 726490a994b26813dfbeb521281ed41aaa2363ad Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 21 Jul 2014 19:15:51 +0100 Subject: bug fix --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 8abf6cf..309cebd 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -950,13 +950,13 @@ namespace OpenSim.Region.Physics.OdePlugin bool noskip = true; if (dop1ava) { - if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision))) + if (!(((OdeCharacter)p1).Collide(g1,g2, false, ref curContact, ref FeetCollision))) noskip = false; } else if (dop2ava) { - if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision))) + if (!(((OdeCharacter)p2).Collide(g2,g1, true, ref curContact, ref FeetCollision))) noskip = false; } @@ -1095,10 +1095,11 @@ namespace OpenSim.Region.Physics.OdePlugin // do colisions with static space d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback); - // chars with chars - d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); // no coll with gnd } + // chars with chars + d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); + } catch (AccessViolationException) { -- cgit v1.1 From d9797b64784d4c4ec1d1d55c7a7bdaa71bfe3ebe Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 21 Jul 2014 19:16:23 +0100 Subject: change ava to ava collisions a bit --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 72 +++++++++++++--------- 1 file changed, 42 insertions(+), 30 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index ecd5474..daf6c7c 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -96,6 +96,8 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_feetOffset = 0; private float feetOff = 0; private float boneOff = 0; + private float AvaAvaSizeXsq = 0.3f; + private float AvaAvaSizeYsq = 0.2f; public float walkDivisor = 1.3f; public float runDivisor = 0.8f; @@ -136,7 +138,6 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr Body = IntPtr.Zero; private OdeScene _parent_scene; private IntPtr capsule = IntPtr.Zero; - private IntPtr bbox = IntPtr.Zero; public IntPtr collider = IntPtr.Zero; public IntPtr Amotor = IntPtr.Zero; @@ -709,6 +710,11 @@ namespace OpenSim.Region.Physics.OdePlugin feetOff = bot + feetsz; + AvaAvaSizeXsq = 0.4f * sx; + AvaAvaSizeXsq *= AvaAvaSizeXsq; + AvaAvaSizeYsq = 0.5f * sy; + AvaAvaSizeYsq *= AvaAvaSizeYsq; + _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); collider = d.HashSpaceCreate(_parent_scene.CharsSpace); @@ -723,9 +729,8 @@ namespace OpenSim.Region.Physics.OdePlugin r = m_size.Y; float l = m_size.Z - r; r *= 0.5f; - capsule = d.CreateCapsule(collider, r, l); - bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z); + capsule = d.CreateCapsule(collider, r, l); m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass @@ -750,8 +755,6 @@ namespace OpenSim.Region.Physics.OdePlugin _position.Z = npositionZ; d.BodySetMass(Body, ref ShellMass); - - d.GeomSetBody(bbox, Body); d.GeomSetBody(capsule, Body); // The purpose of the AMotor here is to keep the avatar's physical @@ -821,14 +824,6 @@ namespace OpenSim.Region.Physics.OdePlugin capsule = IntPtr.Zero; } - if (bbox != IntPtr.Zero) - { - _parent_scene.actor_name_map.Remove(bbox); - _parent_scene.waitForSpaceUnlock(collider); - d.GeomDestroy(bbox); - bbox = IntPtr.Zero; - } - if (collider != IntPtr.Zero) { d.SpaceDestroy(collider); @@ -871,20 +866,44 @@ namespace OpenSim.Region.Physics.OdePlugin } - public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) + public bool Collide(IntPtr me,IntPtr other, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) { feetcollision = false; - Vector3 offset; - - offset.X = contact.pos.X - _position.X; - offset.Y = contact.pos.Y - _position.Y; - if (me == capsule) { + Vector3 offset; + float h = contact.pos.Z - _position.Z; offset.Z = h - feetOff; + offset.X = contact.pos.X - _position.X; + offset.Y = contact.pos.Y - _position.Y; + + d.GeomClassID gtype = d.GeomGetClass(other); + if (gtype == d.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 -(float)Math.Sqrt((double)r); + if (dp > 0.05f) + dp = 0.05f; + + contact.depth = dp; + + if (offset.Z < 0) + { + feetcollision = true; + if (h < boneOff) + IsColliding = true; + } + return true; + } + if (offset.Z > 0) return true; @@ -905,11 +924,9 @@ namespace OpenSim.Region.Physics.OdePlugin feetcollision = true; if (h < boneOff) IsColliding = true; + return true; } - else - return false; - - return true; + return false; } /// @@ -1094,8 +1111,8 @@ namespace OpenSim.Region.Physics.OdePlugin // Avatar to Avatar collisions // Prim to avatar collisions - vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2); - vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2); + vec.X = -vel.X * PID_D * 2 + (_zeroPosition.X - localpos.X) * (PID_P * 5); + vec.Y = -vel.Y * PID_D * 2 + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); if (flying) { vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; @@ -1389,10 +1406,6 @@ namespace OpenSim.Region.Physics.OdePlugin AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); _parent_scene.actor_name_map[collider] = (PhysicsActor)this; -// _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; -// _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; -// _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; - _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; _parent_scene.AddCharacter(this); } @@ -1449,7 +1462,6 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.actor_name_map[collider] = (PhysicsActor)this; - _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; } m_freemove = false; -- cgit v1.1 From 0239c7ba1c85fe67ec5f9657c4193228677349ff Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 21 Jul 2014 23:54:53 +0100 Subject: avatar collisions fix --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 105 ++++++++++++----------- 1 file changed, 54 insertions(+), 51 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 309cebd..9bf2abe 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -833,13 +833,7 @@ namespace OpenSim.Region.Physics.OdePlugin switch (p2.PhysicsActorType) { case (int)ActorTypes.Agent: - p1.CollidingObj = true; - p2.CollidingObj = true; - break; - case (int)ActorTypes.Prim: - if (p2.Velocity.LengthSquared() > 0.0f) - p2.CollidingObj = true; break; default: @@ -850,55 +844,53 @@ namespace OpenSim.Region.Physics.OdePlugin } case (int)ActorTypes.Prim: - switch (p2.PhysicsActorType) { - case (int)ActorTypes.Agent: - - dop2ava = true; - - if (p1.Velocity.LengthSquared() > 0.0f) - p1.CollidingObj = true; - break; + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + dop2ava = true; + break; - case (int)ActorTypes.Prim: - Vector3 relV = p1.Velocity - p2.Velocity; - float relVlenSQ = relV.LengthSquared(); - if (relVlenSQ > 0.0001f) - { - p1.CollidingObj = true; - p2.CollidingObj = true; - } - p1.getContactData(ref contactdata1); - p2.getContactData(ref contactdata2); - bounce = contactdata1.bounce * contactdata2.bounce; - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + case (int)ActorTypes.Prim: + Vector3 relV = p1.Velocity - p2.Velocity; + float relVlenSQ = relV.LengthSquared(); + if (relVlenSQ > 0.0001f) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); + bounce = contactdata1.bounce * contactdata2.bounce; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - if (relVlenSQ > 0.01f) - mu *= frictionMovementMult; + if (relVlenSQ > 0.01f) + mu *= frictionMovementMult; - break; - - case (int)ActorTypes.Ground: - p1.getContactData(ref contactdata1); - bounce = contactdata1.bounce * TerrainBounce; - mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + break; - if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) - mu *= frictionMovementMult; - p1.CollidingGround = true; -/* - if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) - { - if (curContact.side1 > 0) - IgnoreNegSides = true; - } - */ - break; + case (int)ActorTypes.Ground: + p1.getContactData(ref contactdata1); + bounce = contactdata1.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + + if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + p1.CollidingGround = true; + /* + if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) + { + if (curContact.side1 > 0) + IgnoreNegSides = true; + } + */ + break; - case (int)ActorTypes.Water: - default: - ignore = true; - break; + case (int)ActorTypes.Water: + default: + ignore = true; + break; + } } break; @@ -950,14 +942,25 @@ namespace OpenSim.Region.Physics.OdePlugin bool noskip = true; if (dop1ava) { - if (!(((OdeCharacter)p1).Collide(g1,g2, false, ref curContact, ref FeetCollision))) - + if (!(((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref FeetCollision))) noskip = false; + else + { + if(p2.PhysicsActorType == (int)ActorTypes.Agent) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + else if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + } } else if (dop2ava) { if (!(((OdeCharacter)p2).Collide(g2,g1, true, ref curContact, ref FeetCollision))) noskip = false; + else if (p1.Velocity.LengthSquared() > 0.0f) + p1.CollidingObj = true; } if (noskip) -- cgit v1.1 From 992bd703e7e13ad6021220253d6bdfd8b674d1b6 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 23 Sep 2014 00:04:03 +0100 Subject: fix building state logic... --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 4cac0aa..2c3190f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -251,8 +251,8 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_building; } set { - if (value) - m_building = true; +// if (value) +// m_building = true; AddChange(changes.building, value); } } -- cgit v1.1 From 3052a5388954592861e0a55681844115485b6ae7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 29 Sep 2014 20:17:05 +0100 Subject: change avatar physics and motion control. Still not that good :( --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 210 ++++++++++++++++----- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 167 ++++++++++++++-- 2 files changed, 320 insertions(+), 57 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index daf6c7c..4a98df4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -79,6 +79,7 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 _acceleration; private Vector3 m_rotationalVelocity; private Vector3 m_size; + private Vector3 m_collideNormal; private Quaternion m_orientation; private Quaternion m_orientation2D; private float m_mass = 80f; @@ -109,6 +110,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool _zeroFlag = false; + private uint m_localID = 0; public bool m_returnCollisions = false; // taints and their non-tainted counterparts @@ -153,9 +155,7 @@ namespace OpenSim.Region.Physics.OdePlugin public UUID m_uuid; public bool bad = false; - float mu; - - + float mu; public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) { @@ -864,11 +864,12 @@ namespace OpenSim.Region.Physics.OdePlugin x = tx * cos - y * sin; y = tx * sin + y * cos; } - - - public bool Collide(IntPtr me,IntPtr other, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) + + public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact, + ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision) { feetcollision = false; + useAltcontact = false; if (me == capsule) { @@ -899,31 +900,77 @@ namespace OpenSim.Region.Physics.OdePlugin { feetcollision = true; if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; IsColliding = true; + } } return true; - } + } + + d.AABB aabb; + d.GeomGetAABB(other,out aabb); + float othertop = aabb.MaxZ - _position.Z; + + if (offset.Z > 0 || othertop > -feetOff || contact.normal.Z > 0.35f) + { + if (offset.Z <= 0) + { + feetcollision = true; + if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; + IsColliding = true; + } + } - if (offset.Z > 0) + if (contact.normal.Z < 0.2f) + { + contact.normal.Z = 0; + float t = contact.normal.X * contact.normal.X + contact.normal.Y * contact.normal.Y; + if (t > 0) + { + t = 1.0f / t; + contact.normal.X *= t; + contact.normal.Y *= t; + } + } return true; + } + + altContact = contact; + useAltcontact = true; offset.Normalize(); + if (contact.depth > 0.1f) + contact.depth = 0.1f; + if (reverse) { - contact.normal.X = offset.X; - contact.normal.Y = offset.Y; - contact.normal.Z = offset.Z; + altContact.normal.X = offset.X; + altContact.normal.Y = offset.Y; + altContact.normal.Z = offset.Z; } else { - contact.normal.X = -offset.X; - contact.normal.Y = -offset.Y; - contact.normal.Z = -offset.Z; + altContact.normal.X = -offset.X; + altContact.normal.Y = -offset.Y; + altContact.normal.Z = -offset.Z; } + feetcollision = true; if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; IsColliding = true; + } return true; } return false; @@ -1003,6 +1050,9 @@ namespace OpenSim.Region.Physics.OdePlugin Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); float velLengthSquared = vel.LengthSquared(); + + Vector3 ctz = _target_velocity; + float movementdivisor = 1f; //Ubit change divisions into multiplications below if (!m_alwaysRun) @@ -1010,13 +1060,16 @@ namespace OpenSim.Region.Physics.OdePlugin else movementdivisor = 1 / runDivisor; + ctz.X *= movementdivisor; + ctz.Y *= movementdivisor; + //****************************************** // colide with land d.AABB aabb; // d.GeomGetAABB(feetbox, out aabb); d.GeomGetAABB(capsule, out aabb); - float chrminZ = aabb.MinZ; ; // move up a bit + float chrminZ = aabb.MinZ; // move up a bit Vector3 posch = localpos; float ftmp; @@ -1031,15 +1084,18 @@ namespace OpenSim.Region.Physics.OdePlugin float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); if (chrminZ < terrainheight) { + if (ctz.Z < 0) + ctz.Z = 0; + + Vector3 n = _parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y); float depth = terrainheight - chrminZ; + + vec.Z = depth * PID_P * 50; + if (!flying) - { - vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50; - } - else - vec.Z = depth * PID_P * 50; + vec.Z += -vel.Z * PID_D; - if (depth < 0.1f) + if (depth < 0.2f) { m_colliderGroundfilter++; if (m_colliderGroundfilter > 2) @@ -1053,50 +1109,83 @@ namespace OpenSim.Region.Physics.OdePlugin m_freemove = false; } + m_collideNormal.X = n.X; + m_collideNormal.Y = n.Y; + m_collideNormal.Z = n.Z; + m_iscollidingGround = true; + ContactPoint contact = new ContactPoint(); contact.PenetrationDepth = depth; contact.Position.X = localpos.X; contact.Position.Y = localpos.Y; contact.Position.Z = terrainheight; - contact.SurfaceNormal.X = 0.0f; - contact.SurfaceNormal.Y = 0.0f; - contact.SurfaceNormal.Z = -1f; + contact.SurfaceNormal.X = -n.X; + contact.SurfaceNormal.Y = -n.Y; + contact.SurfaceNormal.Z = -n.Z; contact.RelativeSpeed = -vel.Z; contact.CharacterFeet = true; AddCollisionEvent(0, contact); - vec.Z *= 0.5f; +// vec.Z *= 0.5f; } } else { - m_colliderGroundfilter = 0; - m_iscollidingGround = false; + m_colliderGroundfilter -= 5; + if (m_colliderGroundfilter <= 0) + { + m_colliderGroundfilter = 0; + m_iscollidingGround = false; + } } } else { - m_colliderGroundfilter = 0; - m_iscollidingGround = false; + m_colliderGroundfilter -= 5; + if (m_colliderGroundfilter <= 0) + { + m_colliderGroundfilter = 0; + m_iscollidingGround = false; + } } //****************************************** + if (!m_iscolliding) + m_collideNormal.Z = 0; + + bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f); + - bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); - // if (!tviszero || m_iscolliding || velLengthSquared <0.01) if (!tviszero) + { m_freemove = false; + // movement relative to surface if moving on it + // dont disturbe vertical movement, ie jumps + if (m_iscolliding && !flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f) + { + float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y; + ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X); + ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y); + ctz.Z -= p; + if (ctz.Z < 0) + ctz.Z *= 2; + + } + + } + + if (!m_freemove) { // if velocity is zero, use position control; otherwise, velocity control - if (tviszero && m_iscolliding) + if (tviszero && m_iscolliding && !flying) { // keep track of where we stopped. No more slippin' & slidin' if (!_zeroFlag) @@ -1129,22 +1218,48 @@ namespace OpenSim.Region.Physics.OdePlugin { if (!flying) { - if (_target_velocity.Z > 0.0f) + // we are on a surface + if (ctz.Z > 0f) { - // We're colliding with something and we're not flying but we're moving - // This means we're walking or running. JUMPING - vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; + // moving up or JUMPING + vec.Z += (ctz.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); } + else + { + // we are moving down on a surface + if (ctz.Z == 0) + { + if (vel.Z > 0) + vec.Z -= vel.Z * PID_D * 2.0f; + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); + } + // intencionally going down + else + { + if (ctz.Z < vel.Z) + vec.Z += (ctz.Z - vel.Z) * PID_D * 2.0f; + else + { + } + + if (Math.Abs(ctz.X) > Math.Abs(vel.X)) + vec.X += (ctz.X - vel.X) * (PID_D); + if (Math.Abs(ctz.Y) > Math.Abs(vel.Y)) + vec.Y += (ctz.Y - vel.Y) * (PID_D); + } + } + // We're standing on something - vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D); - vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D); } else { // We're flying and colliding with something - vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f); - vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f); - vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f); + vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f); + vec.Z += (ctz.Z - vel.Z) * (PID_D); } } else // ie not colliding @@ -1152,9 +1267,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (flying) //(!m_iscolliding && flying) { // we're in mid air suspended - vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f); - vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f); - vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + vec.X += (ctz.X - vel.X) * (PID_D * 1.667f); + vec.Y += (ctz.Y - vel.Y) * (PID_D * 1.667f); + vec.Z += (ctz.Z - vel.Z) * (PID_D); } else @@ -1163,8 +1278,11 @@ namespace OpenSim.Region.Physics.OdePlugin // m_iscolliding includes collisions with the ground. // d.Vector3 pos = d.BodyGetPosition(Body); - vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f; - vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f; + vec.X += (ctz.X - vel.X) * PID_D * 0.833f; + vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f; + // hack for breaking on fall + if (ctz.Z == -9999f) + vec.Z += -vel.Z * PID_D - _parent_scene.gravityz * m_mass; } } } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 9bf2abe..2adbe01 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -935,18 +935,22 @@ namespace OpenSim.Region.Physics.OdePlugin SharedTmpcontact.surface.mu = mu; SharedTmpcontact.surface.bounce = bounce; + d.ContactGeom altContact = new d.ContactGeom(); + bool useAltcontact = false; + bool noskip = true; + while (true) { // if (!(IgnoreNegSides && curContact.side1 < 0)) { - bool noskip = true; + noskip = true; + useAltcontact = false; + if (dop1ava) { - if (!(((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref FeetCollision))) - noskip = false; - else + if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) { - if(p2.PhysicsActorType == (int)ActorTypes.Agent) + if (p2.PhysicsActorType == (int)ActorTypes.Agent) { p1.CollidingObj = true; p2.CollidingObj = true; @@ -954,18 +958,32 @@ namespace OpenSim.Region.Physics.OdePlugin else if (p2.Velocity.LengthSquared() > 0.0f) p2.CollidingObj = true; } + else + noskip = false; } else if (dop2ava) { - if (!(((OdeCharacter)p2).Collide(g2,g1, true, ref curContact, ref FeetCollision))) + if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) + { + if (p1.PhysicsActorType == (int)ActorTypes.Agent) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + else if (p2.Velocity.LengthSquared() > 0.0f) + p1.CollidingObj = true; + } + else noskip = false; - else if (p1.Velocity.LengthSquared() > 0.0f) - p1.CollidingObj = true; } if (noskip) { - Joint = CreateContacJoint(ref curContact); + if(useAltcontact) + Joint = CreateContacJoint(ref altContact); + else + Joint = CreateContacJoint(ref curContact); + if (Joint == IntPtr.Zero) break; @@ -1924,12 +1942,12 @@ namespace OpenSim.Region.Physics.OdePlugin dy = 0; } } - else { // we still have square fixed size regions // also flip x and y because of how map is done for ODE fliped axis // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) { iy = (int)x; @@ -1976,7 +1994,7 @@ namespace OpenSim.Region.Physics.OdePlugin */ h0 = ((float)heights[iy]); // 0,0 vertice - if ((dy > dx)) + if (dy>dx) { iy += regsize; h2 = (float)heights[iy]; // 0,1 vertice @@ -1994,6 +2012,133 @@ namespace OpenSim.Region.Physics.OdePlugin return h0 + h1 + h2; } + public Vector3 GetTerrainNormalAtXY(float x, float y) + { + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + + IntPtr heightFieldGeom = IntPtr.Zero; + Vector3 norm = new Vector3(0, 0, 1); + + // get region map + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return norm; + + if (heightFieldGeom == IntPtr.Zero) + return norm; + + if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + return norm; + + // TerrainHeightField for ODE as offset 1m + x += 1f - offsetX; + y += 1f - offsetY; + + // make position fit into array + if (x < 0) + x = 0; + if (y < 0) + y = 0; + + // integer indexs + int ix; + int iy; + // interpolators offset + float dx; + float dy; + + + int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples + int xstep = 1; + int ystep = regsize; + bool firstTri = false; + + if (OdeUbitLib) + { + if (x < regsize - 1) + { + ix = (int)x; + dx = x - (float)ix; + } + else // out world use external height + { + ix = regsize - 2; + dx = 0; + } + if (y < regsize - 1) + { + iy = (int)y; + dy = y - (float)iy; + } + else + { + iy = regsize - 2; + dy = 0; + } + firstTri = dy > dx; + } + + else + { + xstep = regsize; + ystep = 1; + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsize - 2; + dy = 0; + } + if (y < regsize - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsize - 2; + dx = 0; + } + firstTri = dx > dy; + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + + if (firstTri) + { + h1 = ((float)heights[iy]); // 0,0 vertice + iy += ystep; + h0 = (float)heights[iy]; // 0,1 + h2 = (float)heights[iy+xstep]; // 1,1 vertice + norm.X = h0 - h2; + norm.Y = h1 - h0; + } + else + { + h2 = ((float)heights[iy]); // 0,0 vertice + iy += xstep; + h0 = ((float)heights[iy]); // 1,0 vertice + h1 = (float)heights[iy+ystep]; // vertice 1,1 + norm.X = h2 - h0; + norm.Y = h0 - h1; + } + norm.Z = 1; + norm.Normalize(); + return norm; + } public override void SetTerrain(float[] heightMap) { -- cgit v1.1 From 1b2e2a86a3e54ef9437697f7460924f64d986b7c Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 29 Sep 2014 23:24:22 +0100 Subject: remove check of other prim top height on steps climb code --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 4a98df4..992fae7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -909,12 +909,13 @@ namespace OpenSim.Region.Physics.OdePlugin } return true; } - +/* d.AABB aabb; d.GeomGetAABB(other,out aabb); float othertop = aabb.MaxZ - _position.Z; - - if (offset.Z > 0 || othertop > -feetOff || contact.normal.Z > 0.35f) +*/ +// if (offset.Z > 0 || othertop > -feetOff || contact.normal.Z > 0.35f) + if (offset.Z > 0 || contact.normal.Z > 0.35f) { if (offset.Z <= 0) { -- cgit v1.1 From 44a42efa4b9e3263226e469f361a798b3b12ed8a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 4 Oct 2014 08:46:46 +0100 Subject: try to help steps climb a bit compensating the bounce reduction --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 2 ++ 1 file changed, 2 insertions(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 992fae7..f94e70b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -946,6 +946,8 @@ namespace OpenSim.Region.Physics.OdePlugin altContact = contact; useAltcontact = true; + offset.Z -= 0.2f; + offset.Normalize(); if (contact.depth > 0.1f) -- cgit v1.1 From c3a1d6b5ef2e889f53ff3b0c1b6b1002d6f9f6b8 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 5 Oct 2014 18:55:37 +0100 Subject: reduce some avatar engine strenght --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index f94e70b..af7ca1d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -928,7 +928,7 @@ namespace OpenSim.Region.Physics.OdePlugin IsColliding = true; } } - +/* if (contact.normal.Z < 0.2f) { contact.normal.Z = 0; @@ -940,6 +940,7 @@ namespace OpenSim.Region.Physics.OdePlugin contact.normal.Y *= t; } } + */ return true; } @@ -1203,8 +1204,8 @@ namespace OpenSim.Region.Physics.OdePlugin // Avatar to Avatar collisions // Prim to avatar collisions - vec.X = -vel.X * PID_D * 2 + (_zeroPosition.X - localpos.X) * (PID_P * 5); - vec.Y = -vel.Y * PID_D * 2 + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); + vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 5); + vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); if (flying) { vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; @@ -1225,7 +1226,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (ctz.Z > 0f) { // moving up or JUMPING - vec.Z += (ctz.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; + vec.Z += (ctz.Z - vel.Z) * PID_D; vec.X += (ctz.X - vel.X) * (PID_D); vec.Y += (ctz.Y - vel.Y) * (PID_D); } @@ -1235,7 +1236,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (ctz.Z == 0) { if (vel.Z > 0) - vec.Z -= vel.Z * PID_D * 2.0f; + vec.Z -= vel.Z * PID_D; vec.X += (ctz.X - vel.X) * (PID_D); vec.Y += (ctz.Y - vel.Y) * (PID_D); } @@ -1243,7 +1244,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { if (ctz.Z < vel.Z) - vec.Z += (ctz.Z - vel.Z) * PID_D * 2.0f; + vec.Z += (ctz.Z - vel.Z) * PID_D; else { } @@ -1262,7 +1263,7 @@ namespace OpenSim.Region.Physics.OdePlugin // We're flying and colliding with something vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f); vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f); - vec.Z += (ctz.Z - vel.Z) * (PID_D); + vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f); } } else // ie not colliding @@ -1270,8 +1271,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (flying) //(!m_iscolliding && flying) { // we're in mid air suspended - vec.X += (ctz.X - vel.X) * (PID_D * 1.667f); - vec.Y += (ctz.Y - vel.Y) * (PID_D * 1.667f); + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); vec.Z += (ctz.Z - vel.Z) * (PID_D); } -- cgit v1.1 From 10d3d0c81de69106cd0a4f4a6d4f94310f595f43 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 12 Oct 2014 01:14:54 +0100 Subject: try to reduce avatar bounce on falls. Not all possible side effects checked, specially on portals --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 44 ++++++++++------------ 1 file changed, 19 insertions(+), 25 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index af7ca1d..1c38246 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -311,9 +311,9 @@ namespace OpenSim.Region.Physics.OdePlugin { if (value) { - m_colliderfilter += 2; - if (m_colliderfilter > 2) - m_colliderfilter = 2; + m_colliderfilter += 3; + if (m_colliderfilter > 3) + m_colliderfilter = 3; } else { @@ -328,6 +328,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_pidControllerActive = true; m_iscolliding = true; + m_freemove = false; } } } @@ -928,19 +929,6 @@ namespace OpenSim.Region.Physics.OdePlugin IsColliding = true; } } -/* - if (contact.normal.Z < 0.2f) - { - contact.normal.Z = 0; - float t = contact.normal.X * contact.normal.X + contact.normal.Y * contact.normal.Y; - if (t > 0) - { - t = 1.0f / t; - contact.normal.X *= t; - contact.normal.Y *= t; - } - } - */ return true; } @@ -1204,12 +1192,18 @@ namespace OpenSim.Region.Physics.OdePlugin // Avatar to Avatar collisions // Prim to avatar collisions - vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 5); - vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); + vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5); + vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); + if(vel.Z > 0) + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + else + vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f; +/* if (flying) { vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; } +*/ } //PidStatus = true; } @@ -1226,7 +1220,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (ctz.Z > 0f) { // moving up or JUMPING - vec.Z += (ctz.Z - vel.Z) * PID_D; + 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); } @@ -1236,7 +1230,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (ctz.Z == 0) { if (vel.Z > 0) - vec.Z -= vel.Z * PID_D; + vec.Z -= vel.Z * PID_D * 2f; vec.X += (ctz.X - vel.X) * (PID_D); vec.Y += (ctz.Y - vel.Y) * (PID_D); } @@ -1305,9 +1299,9 @@ namespace OpenSim.Region.Physics.OdePlugin vec.X -= breakfactor * vel.X; vec.Y -= breakfactor * vel.Y; if (flying) - vec.Z -= breakfactor * vel.Z; + vec.Z -= 0.5f * breakfactor * vel.Z; else - vec.Z -= .5f* m_mass * vel.Z; + vec.Z -= .16f* m_mass * vel.Z; } if (flying) @@ -1687,9 +1681,9 @@ namespace OpenSim.Region.Physics.OdePlugin _zeroFlag = false; _target_velocity = Vector3.Zero; m_freemove = true; - m_colliderfilter = -2; - m_colliderObjectfilter = -2; - m_colliderGroundfilter = -2; + m_colliderfilter = -1; + m_colliderObjectfilter = -1; + m_colliderGroundfilter = -1; m_iscolliding = false; m_iscollidingGround = false; -- cgit v1.1 From 9d52b7ff71579719f0e1f2cb45f0aca30e7d1bec Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 13 Oct 2014 12:01:36 +0100 Subject: name the thread --- OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs | 1 + 1 file changed, 1 insertion(+) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs index 5030cec..a6bdaa0 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs @@ -100,6 +100,7 @@ namespace OpenSim.Region.Physics.OdePlugin } m_running = true; m_thread = new Thread(DoWork); + m_thread.Name = "OdeMeshWorker"; m_thread.Start(); } -- cgit v1.1 From 99668a63e44c0cea2de2123a56c9425aa579e7e7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 13 Feb 2015 13:37:12 +0000 Subject: fix axis locking Amotor --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 2c3190f..ebaa50f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1339,8 +1339,8 @@ namespace OpenSim.Region.Physics.OdePlugin // ODE should do this with axis relative to body 1 but seems to fail d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); d.JointSetAMotorAngle(Amotor, 0, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); @@ -1356,8 +1356,8 @@ namespace OpenSim.Region.Physics.OdePlugin ax = (new Vector3(0, 1, 0)) * curr; d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); @@ -1373,8 +1373,8 @@ namespace OpenSim.Region.Physics.OdePlugin ax = (new Vector3(0, 0, 1)) * curr; d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); -- cgit v1.1 From 07dead7dcb8b0f2a27a50748e4a460d9669903fc Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Sun, 29 Mar 2015 14:25:12 -0700 Subject: varregion: any conversions of use of Constants.RegionSize converted into Util.cs routines to convert region coords to and from world coords or handles. --- .../Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs | 17 ++++++++++++----- OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs | 14 ++++++++++++-- OpenSim/Region/Physics/Manager/PhysicsScene.cs | 9 +++++++++ 3 files changed, 33 insertions(+), 7 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs index 0816b7b..8e40561 100644 --- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs +++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs @@ -46,6 +46,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin private List _actors = new List(); private List _prims = new List(); private float[] _heightMap; + private Vector3 m_regionExtent; //protected internal string sceneIdentifier; @@ -58,6 +59,12 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin public override void Initialise(IMesher meshmerizer, IConfigSource config) { + throw new Exception("Should not be called."); + } + + public override void Initialise(IMesher meshmerizer, IConfigSource config, Vector3 regionExtent) + { + m_regionExtent = regionExtent; } public override void Dispose() {} @@ -121,23 +128,23 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin { actorPosition.Y = 0.1F; } - else if (actor.Position.Y >= Constants.RegionSize) + else if (actor.Position.Y >= m_regionExtent.Y) { - actorPosition.Y = ((int)Constants.RegionSize - 0.1f); + actorPosition.Y = (m_regionExtent.Y - 0.1f); } if (actor.Position.X < 0) { actorPosition.X = 0.1F; } - else if (actor.Position.X >= Constants.RegionSize) + else if (actor.Position.X >= m_regionExtent.X) { - actorPosition.X = ((int)Constants.RegionSize - 0.1f); + actorPosition.X = (m_regionExtent.X - 0.1f); } float terrainHeight = 0; if (_heightMap != null) - terrainHeight = _heightMap[(int)actor.Position.Y * Constants.RegionSize + (int)actor.Position.X]; + terrainHeight = _heightMap[(int)actor.Position.Y * (int)m_regionExtent.Y + (int)actor.Position.X]; float height = terrainHeight + actor.Size.Z; diff --git a/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs b/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs index 8ccfda5..d14edfd 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs @@ -32,6 +32,7 @@ using System.Reflection; using Nini.Config; using log4net; using OpenSim.Framework; +using OpenMetaverse; namespace OpenSim.Region.Physics.Manager { @@ -59,6 +60,14 @@ namespace OpenSim.Region.Physics.Manager m_log.Info("[PHYSICS]: Added meshing engine: " + plugHard.GetName()); } + // Legacy method for simulators before extent was passed + public PhysicsScene GetPhysicsScene(string physEngineName, string meshEngineName, + IConfigSource config, string regionName) + { + Vector3 extent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight); + return GetPhysicsScene(physEngineName, meshEngineName, config, regionName, extent); + } + /// /// Get a physics scene for the given physics engine and mesher. /// @@ -66,7 +75,8 @@ namespace OpenSim.Region.Physics.Manager /// /// /// - public PhysicsScene GetPhysicsScene(string physEngineName, string meshEngineName, IConfigSource config, string regionName) + public PhysicsScene GetPhysicsScene(string physEngineName, string meshEngineName, + IConfigSource config, string regionName, Vector3 regionExtent) { if (String.IsNullOrEmpty(physEngineName)) { @@ -94,7 +104,7 @@ namespace OpenSim.Region.Physics.Manager { m_log.Info("[PHYSICS]: creating " + physEngineName); PhysicsScene result = _PhysPlugins[physEngineName].GetScene(regionName); - result.Initialise(meshEngine, config); + result.Initialise(meshEngine, config, regionExtent); return result; } else diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index dd9bbc1..4f4ff07 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -132,8 +132,17 @@ namespace OpenSim.Region.Physics.Manager } } + // Deprecated. Do not use this for new physics engines. public abstract void Initialise(IMesher meshmerizer, IConfigSource config); + // For older physics engines that do not implement non-legacy region sizes. + // If the physics engine handles the region extent feature, it overrides this function. + public virtual void Initialise(IMesher meshmerizer, IConfigSource config, Vector3 regionExtent) + { + // If not overridden, call the old initialization entry. + Initialise(meshmerizer, config); + } + /// /// Add an avatar /// -- cgit v1.1 From abeb2ec4b30357366314c844ae9609431652e05d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 19 Aug 2015 14:26:14 +0100 Subject: ubitode varsize ( Mega disabled till better checks ) --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 155 +++++++++++++++-------- 1 file changed, 101 insertions(+), 54 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 2adbe01..f2b21a3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -170,6 +170,8 @@ namespace OpenSim.Region.Physics.OdePlugin // private Dictionary m_storedCollisions = new Dictionary(); public bool OdeUbitLib = false; + public bool m_suportCombine = false; // mega suport not tested + // private int threadid = 0; // private Random fluidRandomizer = new Random(Environment.TickCount); @@ -187,8 +189,8 @@ namespace OpenSim.Region.Physics.OdePlugin public float AvatarFriction = 0;// 0.9f * 0.5f; - private const uint m_regionWidth = Constants.RegionSize; - private const uint m_regionHeight = Constants.RegionSize; + private uint m_regionWidth = Constants.RegionSize; + private uint m_regionHeight = Constants.RegionSize; public float ODE_STEPSIZE = 0.020f; public float HalfOdeStep = 0.01f; @@ -418,8 +420,15 @@ namespace OpenSim.Region.Physics.OdePlugin } } - // Initialize the mesh plugin -// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region ) + public override void Initialise(IMesher meshmerizer, IConfigSource config, Vector3 regionExtent) + { + WorldExtents.X = regionExtent.X; + WorldExtents.Y = regionExtent.Y; + m_suportCombine = false; + Initialise(meshmerizer, config); + } + + public override void Initialise(IMesher meshmerizer, IConfigSource config) { // checkThread(); @@ -557,6 +566,11 @@ namespace OpenSim.Region.Physics.OdePlugin spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter); + if (spaceGridMaxX > 40) + spaceGridMaxX = 40; + if (spaceGridMaxY > 40) + spaceGridMaxY = 40; + staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; // create all spaces now @@ -1882,17 +1896,25 @@ namespace OpenSim.Region.Physics.OdePlugin public float GetTerrainHeightAtXY(float x, float y) { + IntPtr heightFieldGeom = IntPtr.Zero; + int offsetX = 0; + int offsetY = 0; - int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - - - IntPtr heightFieldGeom = IntPtr.Zero; + if (m_suportCombine) + { + offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + // get region map + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return 0f; + } + else + { + if (!RegionTerrain.TryGetValue(Vector3.Zero , out heightFieldGeom)) + return 0f; + } - // get region map - if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) - return 0f; if (heightFieldGeom == IntPtr.Zero) return 0f; @@ -1917,28 +1939,30 @@ namespace OpenSim.Region.Physics.OdePlugin float dx; float dy; - int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples + int regsizeX = (int)WorldExtents.X + 3; // map size see setterrain number of samples + int regsizeY = (int)WorldExtents.Y + 3; // map size see setterrain number of samples + int regsize = regsizeX; if (OdeUbitLib) { - if (x < regsize - 1) + if (x < regsizeX - 1) { ix = (int)x; dx = x - (float)ix; } else // out world use external height { - ix = regsize - 2; + ix = regsizeX - 2; dx = 0; } - if (y < regsize - 1) + if (y < regsizeY - 1) { iy = (int)y; dy = y - (float)iy; } else { - iy = regsize - 2; + iy = regsizeY - 2; dy = 0; } } @@ -1948,24 +1972,26 @@ namespace OpenSim.Region.Physics.OdePlugin // also flip x and y because of how map is done for ODE fliped axis // so ix,iy,dx and dy are inter exchanged - if (x < regsize - 1) + regsize = regsizeY; + + if (x < regsizeX - 1) { iy = (int)x; dy = x - (float)iy; } else // out world use external height { - iy = regsize - 2; + iy = regsizeX - 2; dy = 0; } - if (y < regsize - 1) + if (y < regsizeY - 1) { ix = (int)y; dx = y - (float)ix; } else { - ix = regsize - 2; + ix = regsizeY - 2; dx = 0; } } @@ -2014,15 +2040,25 @@ namespace OpenSim.Region.Physics.OdePlugin public Vector3 GetTerrainNormalAtXY(float x, float y) { - int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - IntPtr heightFieldGeom = IntPtr.Zero; Vector3 norm = new Vector3(0, 0, 1); - // get region map - if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) - return norm; + int offsetX = 0; + int offsetY = 0; + + if (m_suportCombine) + { + offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + // get region map + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return norm; ; + } + else + { + if (!RegionTerrain.TryGetValue(Vector3.Zero, out heightFieldGeom)) + return norm; ; + } if (heightFieldGeom == IntPtr.Zero) return norm; @@ -2047,32 +2083,34 @@ namespace OpenSim.Region.Physics.OdePlugin float dx; float dy; + int regsizeX = (int)WorldExtents.X + 3; // map size see setterrain number of samples + int regsizeY = (int)WorldExtents.Y + 3; // map size see setterrain number of samples + int regsize = regsizeX; - int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples int xstep = 1; - int ystep = regsize; + int ystep = regsizeX; bool firstTri = false; if (OdeUbitLib) { - if (x < regsize - 1) + if (x < regsizeX - 1) { ix = (int)x; dx = x - (float)ix; } else // out world use external height { - ix = regsize - 2; + ix = regsizeX - 2; dx = 0; } - if (y < regsize - 1) + if (y < regsizeY - 1) { iy = (int)y; dy = y - (float)iy; } else { - iy = regsize - 2; + iy = regsizeY - 2; dy = 0; } firstTri = dy > dx; @@ -2080,29 +2118,31 @@ namespace OpenSim.Region.Physics.OdePlugin else { - xstep = regsize; + xstep = regsizeY; ystep = 1; + regsize = regsizeY; + // we still have square fixed size regions // also flip x and y because of how map is done for ODE fliped axis // so ix,iy,dx and dy are inter exchanged - if (x < regsize - 1) + if (x < regsizeX - 1) { iy = (int)x; dy = x - (float)iy; } else // out world use external height { - iy = regsize - 2; + iy = regsizeX - 2; dy = 0; } - if (y < regsize - 1) + if (y < regsizeY - 1) { ix = (int)y; dx = y - (float)ix; } else { - ix = regsize - 2; + ix = regsizeY - 2; dx = 0; } firstTri = dx > dy; @@ -2175,8 +2215,12 @@ namespace OpenSim.Region.Physics.OdePlugin float[] _heightmap; - uint heightmapWidth = Constants.RegionSize + 2; - uint heightmapHeight = Constants.RegionSize + 2; + uint regionsizeX = (uint)WorldExtents.X; + uint regionsizeY = (uint)WorldExtents.Y; + + // map is rotated + uint heightmapWidth = regionsizeY + 2; + uint heightmapHeight = regionsizeX + 2; uint heightmapWidthSamples = heightmapWidth + 1; uint heightmapHeightSamples = heightmapHeight + 1; @@ -2188,7 +2232,6 @@ namespace OpenSim.Region.Physics.OdePlugin const float thickness = 10f; const int wrap = 0; - uint regionsize = Constants.RegionSize; float hfmin = float.MaxValue; float hfmax = float.MinValue; @@ -2196,7 +2239,8 @@ namespace OpenSim.Region.Physics.OdePlugin uint xx; uint yy; - uint maxXXYY = regionsize - 1; + uint maxXX = regionsizeX - 1; + uint maxYY = regionsizeY - 1; // flipping map adding one margin all around so things don't fall in edges uint xt = 0; @@ -2204,13 +2248,13 @@ namespace OpenSim.Region.Physics.OdePlugin for (uint x = 0; x < heightmapWidthSamples; x++) { - if (x > 1 && xx < maxXXYY) + if (x > 1 && xx < maxXX) xx++; yy = 0; for (uint y = 0; y < heightmapHeightSamples; y++) { - if (y > 1 && y < maxXXYY) - yy += regionsize; + if (y > 1 && y < maxYY) + yy += regionsizeX; val = heightMap[yy + xx]; if (val < 0.0f) @@ -2224,6 +2268,7 @@ namespace OpenSim.Region.Physics.OdePlugin } xt += heightmapHeightSamples; } + lock (OdeLock) { IntPtr GroundGeom = IntPtr.Zero; @@ -2296,8 +2341,11 @@ namespace OpenSim.Region.Physics.OdePlugin float[] _heightmap; - uint heightmapWidth = Constants.RegionSize + 2; - uint heightmapHeight = Constants.RegionSize + 2; + uint regionsizeX = (uint)WorldExtents.X; + uint regionsizeY = (uint)WorldExtents.Y; + + uint heightmapWidth = regionsizeX + 2; + uint heightmapHeight = regionsizeY + 2; uint heightmapWidthSamples = heightmapWidth + 1; uint heightmapHeightSamples = heightmapHeight + 1; @@ -2305,14 +2353,13 @@ namespace OpenSim.Region.Physics.OdePlugin _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; - uint regionsize = Constants.RegionSize; - float hfmin = float.MaxValue; // float hfmax = float.MinValue; float val; - uint maxXXYY = regionsize - 1; + uint maxXX = regionsizeX - 1; + uint maxYY = regionsizeY - 1; // adding one margin all around so things don't fall in edges uint xx; @@ -2321,12 +2368,12 @@ namespace OpenSim.Region.Physics.OdePlugin for (uint y = 0; y < heightmapHeightSamples; y++) { - if (y > 1 && y < maxXXYY) - yy += regionsize; + if (y > 1 && y < maxYY) + yy += regionsizeX; xx = 0; for (uint x = 0; x < heightmapWidthSamples; x++) { - if (x > 1 && x < maxXXYY) + if (x > 1 && x < maxXX) xx++; val = heightMap[yy + xx]; @@ -2409,7 +2456,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool SupportsCombining() { - return true; + return m_suportCombine; } /* public override void UnCombine(PhysicsScene pScene) -- cgit v1.1 From ac83525bdef7ce1ae00ff1286f17bdb0f96daa42 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 19 Aug 2015 19:55:24 +0100 Subject: another funny bug (mine this time) --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index f2b21a3..6612e97 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -2326,7 +2326,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); + d.GeomSetPosition(GroundGeom, pOffset.X + WorldExtents.X * 0.5f, pOffset.Y + WorldExtents.Y * 0.5f, 0); RegionTerrain.Add(pOffset, GroundGeom); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); @@ -2436,7 +2436,7 @@ namespace OpenSim.Region.Physics.OdePlugin // geom_name_map[GroundGeom] = "Terrain"; - d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); + d.GeomSetPosition(GroundGeom, pOffset.X + WorldExtents.X * 0.5f, pOffset.Y + WorldExtents.Y * 0.5f, 0); RegionTerrain.Add(pOffset, GroundGeom); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); -- cgit v1.1 From 43a83da5ce4e26d21efe0fb06ba5e6d7a49e39c1 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 20 Aug 2015 13:45:45 +0100 Subject: update old git to os core version --- OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs | 2 +- OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 58 ++++++++++++------------ OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 44 +++++++++--------- OpenSim/Region/Physics/OdePlugin/OdeScene.cs | 11 ++--- 4 files changed, 55 insertions(+), 60 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs index 288cab5..076da78 100644 --- a/OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs +++ b/OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs @@ -55,4 +55,4 @@ using System.Runtime.InteropServices; // You can specify all values by your own or you can build default build and revision // numbers with the '*' character (the default): -[assembly : AssemblyVersion("0.8.0.*")] +[assembly : AssemblyVersion("0.8.2.*")] diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs index 86735de..05eaf2a 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -167,6 +167,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// /// /// + /// /// /// /// @@ -178,7 +179,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// /// public OdeCharacter( - String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, + String avName, OdeScene parent_scene, Vector3 pos, Vector3 vel, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float walk_divisor, float rundivisor) { @@ -210,6 +211,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.WarnFormat("[ODE CHARACTER]: Got NaN Position on Character Create for {0}", avName); } + _velocity = vel; + m_taintTargetVelocity = vel; + _parent_scene = parent_scene; PID_D = pid_d; @@ -500,8 +504,10 @@ namespace OpenSim.Region.Physics.OdePlugin { m_pidControllerActive = true; - m_tainted_CAPSULE_LENGTH = (size.Z) - CAPSULE_RADIUS * 2.0f; -// m_log.Info("[ODE CHARACTER]: " + CAPSULE_LENGTH); + m_tainted_CAPSULE_LENGTH = size.Z - CAPSULE_RADIUS * 2.0f; + + // m_log.InfoFormat("[ODE CHARACTER]: Size = {0}, Capsule Length = {1} (Capsule Radius = {2})", + // size, m_tainted_CAPSULE_LENGTH, CAPSULE_RADIUS); } else { @@ -890,42 +896,30 @@ namespace OpenSim.Region.Physics.OdePlugin // vec, _target_velocity, movementdivisor, vel); } - if (m_iscolliding && !flying && _target_velocity.Z > 0.0f) + if (flying) { - // We're colliding with something and we're not flying but we're moving - // This means we're walking or running. - d.Vector3 pos = d.BodyGetPosition(Body); - vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P; - if (_target_velocity.X > 0) - { - vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; - } - if (_target_velocity.Y > 0) - { - vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; - } + // This also acts as anti-gravity so that we hover when flying rather than fall. + vec.Z = (_target_velocity.Z - vel.Z) * (PID_D); } - else if (!m_iscolliding && !flying) + else { - // we're not colliding and we're not flying so that means we're falling! - // m_iscolliding includes collisions with the ground. - - // d.Vector3 pos = d.BodyGetPosition(Body); - if (_target_velocity.X > 0) + if (m_iscolliding && _target_velocity.Z > 0.0f) { + // We're colliding with something and we're not flying but we're moving + // This means we're walking or running. + d.Vector3 pos = d.BodyGetPosition(Body); + vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; + vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; } - if (_target_velocity.Y > 0) + else if (!m_iscolliding) { + // we're not colliding and we're not flying so that means we're falling! + // m_iscolliding includes collisions with the ground. + vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; } } - - if (flying) - { - // This also acts as anti-gravity so that we hover when flying rather than fall. - vec.Z = (_target_velocity.Z - vel.Z) * (PID_D); - } } if (flying) @@ -1255,7 +1249,11 @@ namespace OpenSim.Region.Physics.OdePlugin } public override Vector3 PIDTarget { set { return; } } - public override bool PIDActive { set { return; } } + public override bool PIDActive + { + get { return false; } + set { return; } + } public override float PIDTau { set { return; } } public override float PIDHoverHeight { set { return; } } diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index b4b7e7f..f934b8a 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -114,7 +114,6 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_PIDTau; private float PID_D = 35f; private float PID_G = 25f; - private bool m_usePID; // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), // and are for non-VEHICLES only. @@ -351,11 +350,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); + d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); } else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } @@ -425,7 +423,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); + d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); } else { @@ -787,6 +785,14 @@ namespace OpenSim.Region.Physics.OdePlugin } } + private void setAngularVelocity(float x, float y, float z) + { + if (Body != (IntPtr)0) + { + d.BodySetAngularVel(Body, x, y, z); + } + } + /// /// Stop a prim from being subject to physics. /// @@ -858,11 +864,6 @@ namespace OpenSim.Region.Physics.OdePlugin private static Dictionary m_MeshToTriMeshMap = new Dictionary(); - public int BadAssetColideBits() - { - return (m_isphysical ? (int)CollisionCategories.Land : 0); - } - private void setMesh(OdeScene parent_scene, IMesh mesh) { // m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh); @@ -1144,7 +1145,7 @@ Console.WriteLine("ZProcessTaints for " + Name); if (prm.m_assetFailed) { d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, prm.BadAssetColideBits()); + d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits); } else { @@ -1198,7 +1199,7 @@ Console.WriteLine("ZProcessTaints for " + Name); if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); + d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); } else { @@ -1400,7 +1401,7 @@ Console.WriteLine("ZProcessTaints for " + Name); if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); + d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); } else { @@ -1729,7 +1730,7 @@ Console.WriteLine(" JointCreateFixed"); // gravityz multiplier = 1 - m_buoyancy fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; - if (m_usePID) + if (PIDActive) { //Console.WriteLine("PID " + Name); // KF - this is for object move? eg. llSetPos() ? @@ -1798,10 +1799,10 @@ Console.WriteLine(" JointCreateFixed"); fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); } - } // end if (m_usePID) + } // end if (PIDActive) // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - if (m_useHoverPID && !m_usePID) + if (m_useHoverPID && !PIDActive) { //Console.WriteLine("Hover " + Name); @@ -2144,7 +2145,7 @@ Console.WriteLine(" JointCreateFixed"); } if (m_assetFailed) - d.GeomSetCollideBits(prim_geom, BadAssetColideBits()); + d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); else d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); @@ -2652,6 +2653,7 @@ Console.WriteLine(" JointCreateFixed"); if (value.IsFinite()) { m_rotationalVelocity = value; + setAngularVelocity(value.X, value.Y, value.Z); } else { @@ -2872,7 +2874,7 @@ Console.WriteLine(" JointCreateFixed"); // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles // adding these logical exclusion situations to maintain this where I think it was intended to be. - if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) + if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) { m_minvelocity = 0.5f; } @@ -2953,9 +2955,9 @@ Console.WriteLine(" JointCreateFixed"); m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); } } - public override bool PIDActive { set { m_usePID = value; } } + public override bool PIDActive { get; set; } public override float PIDTau { set { m_PIDTau = value; } } - + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } public override bool PIDHoverActive { set { m_useHoverPID = value; } } public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } @@ -3350,7 +3352,7 @@ Console.WriteLine(" JointCreateFixed"); RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod; if (assetProvider != null) assetProvider(_pbs.SculptTexture, MeshAssetReceived); - }); + }, null, "ODEPrim.CheckMeshAsset"); } } @@ -3382,4 +3384,4 @@ Console.WriteLine(" JointCreateFixed"); } } } -} +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs index 6d7f079..5953557 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs @@ -1969,16 +1969,11 @@ namespace OpenSim.Region.Physics.OdePlugin #region Add/Remove Entities - public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) - { - Vector3 pos; - pos.X = position.X; - pos.Y = position.Y; - pos.Z = position.Z; - + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) + { OdeCharacter newAv = new OdeCharacter( - avName, this, pos, size, avPIDD, avPIDP, + avName, this, position, velocity, size, avPIDD, avPIDP, avCapRadius, avStandupTensor, avDensity, avMovementDivisorWalk, avMovementDivisorRun); -- cgit v1.1 From e312a0bc902f11f42faecce275d52dc388a1debb Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 20 Aug 2015 14:18:17 +0100 Subject: make it compatible with avn, no point making avn compatible with it --- OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 3 ++- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 17 ++++++++++++----- OpenSim/Region/Physics/OdePlugin/OdeScene.cs | 16 ++++++++++++++++ 3 files changed, 30 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs index 05eaf2a..43bef3c 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -1251,7 +1251,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override Vector3 PIDTarget { set { return; } } public override bool PIDActive { - get { return false; } + // os version + // get { return false; } set { return; } } public override float PIDTau { set { return; } } diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index f934b8a..218ccda 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -114,6 +114,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_PIDTau; private float PID_D = 35f; private float PID_G = 25f; + private bool m_usePID; // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), // and are for non-VEHICLES only. @@ -1730,7 +1731,7 @@ Console.WriteLine(" JointCreateFixed"); // gravityz multiplier = 1 - m_buoyancy fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; - if (PIDActive) + if (m_usePID) { //Console.WriteLine("PID " + Name); // KF - this is for object move? eg. llSetPos() ? @@ -1802,7 +1803,7 @@ Console.WriteLine(" JointCreateFixed"); } // end if (PIDActive) // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - if (m_useHoverPID && !PIDActive) + if (m_useHoverPID && !m_usePID) { //Console.WriteLine("Hover " + Name); @@ -2874,7 +2875,7 @@ Console.WriteLine(" JointCreateFixed"); // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles // adding these logical exclusion situations to maintain this where I think it was intended to be. - if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) + if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) { m_minvelocity = 0.5f; } @@ -2955,7 +2956,10 @@ Console.WriteLine(" JointCreateFixed"); m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); } } - public override bool PIDActive { get; set; } + // os version + //public override bool PIDActive {get { return m_usePID; } set { m_usePID = value; } } + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } @@ -3352,7 +3356,10 @@ Console.WriteLine(" JointCreateFixed"); RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod; if (assetProvider != null) assetProvider(_pbs.SculptTexture, MeshAssetReceived); - }, null, "ODEPrim.CheckMeshAsset"); + // os version + //}, null, "ODEPrim.CheckMeshAsset"); + // avn + }); } } diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs index 5953557..1b7d2bf 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs @@ -1969,6 +1969,7 @@ namespace OpenSim.Region.Physics.OdePlugin #region Add/Remove Entities +/* core version public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) { OdeCharacter newAv @@ -1983,6 +1984,21 @@ namespace OpenSim.Region.Physics.OdePlugin return newAv; } +*/ + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) + { + OdeCharacter newAv + = new OdeCharacter( + avName, this, position, Vector3.Zero, size, avPIDD, avPIDP, + avCapRadius, avStandupTensor, avDensity, + avMovementDivisorWalk, avMovementDivisorRun); + + newAv.Flying = isFlying; + newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; + newAv.m_avatarplanted = avplanted; + + return newAv; + } public override void RemoveAvatar(PhysicsActor actor) { -- cgit v1.1 From 379362facfbc8c50660d546520d4ea78257d04ae Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 20 Aug 2015 17:04:37 +0100 Subject: make basic ode work with var regions for contribution back to CORE. ( avn partial compatibility code needs to be reverted). Cant fully test. --- .../Physics/OdePlugin/ODERayCastRequestManager.cs | 14 +- OpenSim/Region/Physics/OdePlugin/OdeScene.cs | 285 ++++++++++----------- 2 files changed, 147 insertions(+), 152 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs index 8d7d3b3..fa2ed3e 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs @@ -172,8 +172,13 @@ namespace OpenSim.Region.Physics.OdePlugin /// private void RayCast(ODERayCastRequest req) { + // limit ray lenght or collisions will take all avaiable stack space + float len = req.length; + if (len > 250f) + len = 250f; + // Create the ray - IntPtr ray = d.CreateRay(m_scene.space, req.length); + IntPtr ray = d.CreateRay(m_scene.space, len); d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); // Collide test @@ -218,8 +223,13 @@ namespace OpenSim.Region.Physics.OdePlugin /// private void RayCast(ODERayRequest req) { + // limit ray lenght or collisions will take all avaiable stack space + float len = req.length; + if (len > 250f) + len = 250f; + // Create the ray - IntPtr ray = d.CreateRay(m_scene.space, req.length); + IntPtr ray = d.CreateRay(m_scene.space, len); d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); // Collide test diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs index 1b7d2bf..820e649 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs @@ -25,6 +25,14 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +// Ubit changes for varsize regions +// using a large Heightfield geometry for terrain +// ODE ode should handle it fine +// EXCEPT raycasts, those need to have limited range +// (even in normal regions) +// or aplication stack will just blowup + + //#define USE_DRAWSTUFF //#define SPAM @@ -263,8 +271,10 @@ namespace OpenSim.Region.Physics.OdePlugin private Random fluidRandomizer = new Random(Environment.TickCount); - private const uint m_regionWidth = Constants.RegionSize; - private const uint m_regionHeight = Constants.RegionSize; + public bool m_suportCombine = true; + + private uint m_regionWidth = Constants.RegionSize; + private uint m_regionHeight = Constants.RegionSize; private float ODE_STEPSIZE = 0.0178f; private float metersInSpace = 29.9f; @@ -290,7 +300,7 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly IntPtr contactgroup; - internal IntPtr WaterGeom; +// internal IntPtr WaterGeom; private float nmTerrainContactFriction = 255.0f; private float nmTerrainContactBounce = 0.1f; @@ -507,17 +517,17 @@ namespace OpenSim.Region.Physics.OdePlugin public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f); public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f); - // TODO: unused: private uint heightmapWidth = m_regionWidth + 1; - // TODO: unused: private uint heightmapHeight = m_regionHeight + 1; - // TODO: unused: private uint heightmapWidthSamples; - // TODO: unused: private uint heightmapHeightSamples; - private volatile int m_global_contactcount = 0; private Vector3 m_worldOffset = Vector3.Zero; public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); private PhysicsScene m_parentScene = null; + float spacesPerMeterX; + float spacesPerMeterY; + int spaceGridMaxX; + int spaceGridMaxY; + private ODERayCastRequestManager m_rayCastManager; /// @@ -551,7 +561,7 @@ namespace OpenSim.Region.Physics.OdePlugin viewthread.Start(); #endif - _watermap = new float[258 * 258]; + // _watermap = new float[258 * 258]; // Zero out the prim spaces array (we split our space into smaller spaces so // we can hit test less. @@ -572,6 +582,16 @@ namespace OpenSim.Region.Physics.OdePlugin } #endif + public override void Initialise(IMesher meshmerizer, IConfigSource config, Vector3 regionExtent) + { + WorldExtents.X = regionExtent.X; + m_regionWidth = (uint)regionExtent.X; + WorldExtents.Y = regionExtent.Y; + m_regionHeight = (uint)regionExtent.Y; + m_suportCombine = false; + Initialise(meshmerizer, config); + } + // Initialize the mesh plugin public override void Initialise(IMesher meshmerizer, IConfigSource config) { @@ -699,7 +719,31 @@ namespace OpenSim.Region.Physics.OdePlugin contacts = new d.ContactGeom[contactsPerCollision]; - staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)]; + spacesPerMeterX = 1.0f / metersInSpace; + spacesPerMeterY = 1.0f / metersInSpace; + + spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX); + spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY); + + // ubit: limit number of spaces + if (spaceGridMaxX > 40) + { + spaceGridMaxX = 40; + spacesPerMeterX = WorldExtents.X / spaceGridMaxX; + } + if (spaceGridMaxY > 40) + { + spaceGridMaxY = 40; + spacesPerMeterY = WorldExtents.X / spaceGridMaxY; + } + + staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; + + // make this index limits + spaceGridMaxX--; + spaceGridMaxY--; + + // Centeral contact friction and bounce // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why @@ -1883,6 +1927,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) { + if (!m_suportCombine) + return; m_worldOffset = offset; WorldExtents = new Vector2(extents.X, extents.Y); m_parentScene = pScene; @@ -1891,12 +1937,17 @@ namespace OpenSim.Region.Physics.OdePlugin // Recovered for use by fly height. Kitto Flora internal float GetTerrainHeightAtXY(float x, float y) { - int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - IntPtr heightFieldGeom = IntPtr.Zero; + int offsetX = 0; + int offsetY = 0; - if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom)) + if (m_suportCombine) + { + offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + } + + if(RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom)) { if (heightFieldGeom != IntPtr.Zero) { @@ -1910,8 +1961,8 @@ namespace OpenSim.Region.Physics.OdePlugin (int)x < 0.001f || (int)y < 0.001f) return 0; - x = x - offsetX; - y = y - offsetY; + x = x - offsetX + 1f; + y = y - offsetY + 1f; index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y); @@ -2763,16 +2814,16 @@ namespace OpenSim.Region.Physics.OdePlugin { int[] returnint = new int[2]; - returnint[0] = (int) (pos.X/metersInSpace); + returnint[0] = (int) (pos.X * spacesPerMeterX); - if (returnint[0] > ((int) (259f/metersInSpace))) - returnint[0] = ((int) (259f/metersInSpace)); + if (returnint[0] > spaceGridMaxX) + returnint[0] = spaceGridMaxX; if (returnint[0] < 0) returnint[0] = 0; - returnint[1] = (int) (pos.Y/metersInSpace); - if (returnint[1] > ((int) (259f/metersInSpace))) - returnint[1] = ((int) (259f/metersInSpace)); + returnint[1] = (int)(pos.Y * spacesPerMeterY); + if (returnint[1] > spaceGridMaxY) + returnint[1] = spaceGridMaxY; if (returnint[1] < 0) returnint[1] = 0; @@ -3526,6 +3577,7 @@ namespace OpenSim.Region.Physics.OdePlugin get { return false; } } +/* godd try.. but not a fix #region ODE Specific Terrain Fixes private float[] ResizeTerrain512NearestNeighbour(float[] heightMap) { @@ -3792,7 +3844,7 @@ namespace OpenSim.Region.Physics.OdePlugin } #endregion - +*/ public override void SetTerrain(float[] heightMap) { if (m_worldOffset != Vector3.Zero && m_parentScene != null) @@ -3813,71 +3865,63 @@ namespace OpenSim.Region.Physics.OdePlugin int startTime = Util.EnvironmentTickCount(); m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", Name, pOffset); - // this._heightmap[i] = (double)heightMap[i]; - // dbm (danx0r) -- creating a buffer zone of one extra sample all around - //_origheightmap = heightMap; - + float[] _heightmap; - // zero out a heightmap array float array (single dimension [flattened])) - //if ((int)Constants.RegionSize == 256) - // _heightmap = new float[514 * 514]; - //else - - _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; - - uint heightmapWidth = Constants.RegionSize + 1; - uint heightmapHeight = Constants.RegionSize + 1; - - uint heightmapWidthSamples; + // ok im lasy this are just a aliases + uint regionsizeX = m_regionWidth; + uint regionsizeY = m_regionHeight; - uint heightmapHeightSamples; + // map is rotated + uint heightmapWidth = regionsizeY + 2; + uint heightmapHeight = regionsizeX + 2; - //if (((int)Constants.RegionSize) == 256) - //{ - // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2; - // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2; - // heightmapWidth++; - // heightmapHeight++; - //} - //else - //{ + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; - heightmapWidthSamples = (uint)Constants.RegionSize + 1; - heightmapHeightSamples = (uint)Constants.RegionSize + 1; - //} + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; const float scale = 1.0f; const float offset = 0.0f; - const float thickness = 0.2f; + const float thickness = 10f; const int wrap = 0; - int regionsize = (int) Constants.RegionSize + 2; - //Double resolution - //if (((int)Constants.RegionSize) == 256) - // heightMap = ResizeTerrain512Interpolation(heightMap); + float hfmin = float.MaxValue; + float hfmax = float.MinValue; + float val; + uint xx; + uint yy; - // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256) - // regionsize = 512; + uint maxXX = regionsizeX - 1; + uint maxYY = regionsizeY - 1; - float hfmin = 2000; - float hfmax = -2000; - - for (int x = 0; x < heightmapWidthSamples; x++) + // flipping map adding one margin all around so things don't fall in edges + + uint xt = 0; + xx = 0; + + for (uint x = 0; x < heightmapWidthSamples; x++) { - for (int y = 0; y < heightmapHeightSamples; y++) + if (x > 1 && xx < maxXX) + xx++; + yy = 0; + for (uint y = 0; y < heightmapHeightSamples; y++) { - int xx = Util.Clip(x - 1, 0, regionsize - 1); - int yy = Util.Clip(y - 1, 0, regionsize - 1); - - - float val= heightMap[yy * (int)Constants.RegionSize + xx]; - _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val; - - hfmin = (val < hfmin) ? val : hfmin; - hfmax = (val > hfmax) ? val : hfmax; + if (y > 1 && y < maxYY) + yy += regionsizeX; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; + _heightmap[xt + y] = val; + + if (hfmin > val) + hfmin = val; + if (hfmax < val) + hfmax = val; } + xt += heightmapHeightSamples; } lock (OdeLock) @@ -3898,9 +3942,10 @@ namespace OpenSim.Region.Physics.OdePlugin } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1, - (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale, + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth, heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, offset, thickness, wrap); + d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); GroundGeom = d.CreateHeightfield(space, HeightmapData, 1); if (GroundGeom != IntPtr.Zero) @@ -3915,17 +3960,15 @@ namespace OpenSim.Region.Physics.OdePlugin Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); - //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); q1 = q1 * q2; - //q1 = q1 * q3; Vector3 v3; float angle; q1.GetAxisAngle(out v3, out angle); d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)), (pOffset.Y + ((int)Constants.RegionSize * 0.5f)), 0); + d.GeomSetPosition(GroundGeom, pOffset.X + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 0.5f, 0); IntPtr testGround = IntPtr.Zero; if (RegionTerrain.TryGetValue(pOffset, out testGround)) { @@ -3950,89 +3993,31 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool SupportsCombining() { - return true; + return m_suportCombine; } -// public override void UnCombine(PhysicsScene pScene) -// { -// IntPtr localGround = IntPtr.Zero; -//// float[] localHeightfield; -// bool proceed = false; -// List geomDestroyList = new List(); -// -// lock (OdeLock) -// { -// if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) -// { -// foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) -// { -// if (geom == localGround) -// { -//// localHeightfield = TerrainHeightFieldHeights[geom]; -// proceed = true; -// } -// else -// { -// geomDestroyList.Add(geom); -// } -// } -// -// if (proceed) -// { -// m_worldOffset = Vector3.Zero; -// WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); -// m_parentScene = null; -// -// foreach (IntPtr g in geomDestroyList) -// { -// // removingHeightField needs to be done or the garbage collector will -// // collect the terrain data before we tell ODE to destroy it causing -// // memory corruption -// if (TerrainHeightFieldHeights.ContainsKey(g)) -// { -//// float[] removingHeightField = TerrainHeightFieldHeights[g]; -// TerrainHeightFieldHeights.Remove(g); -// -// if (RegionTerrain.ContainsKey(g)) -// { -// RegionTerrain.Remove(g); -// } -// -// d.GeomDestroy(g); -// //removingHeightField = new float[0]; -// } -// } -// -// } -// else -// { -// m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); -// } -// } -// } -// } - public override void SetWaterLevel(float baseheight) { waterlevel = baseheight; - randomizeWater(waterlevel); +// randomizeWater(waterlevel); } +/* private void randomizeWater(float baseheight) { - const uint heightmapWidth = m_regionWidth + 2; - const uint heightmapHeight = m_regionHeight + 2; - const uint heightmapWidthSamples = m_regionWidth + 2; - const uint heightmapHeightSamples = m_regionHeight + 2; - const float scale = 1.0f; - const float offset = 0.0f; - const float thickness = 2.9f; - const int wrap = 0; + uint heightmapWidth = m_regionWidth + 2; + uint heightmapHeight = m_regionHeight + 2; + uint heightmapWidthSamples = m_regionWidth + 2; + uint heightmapHeightSamples = m_regionHeight + 2; + float scale = 1.0f; + float offset = 0.0f; + float thickness = 2.9f; + int wrap = 0; for (int i = 0; i < (258 * 258); i++) { _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); - // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); + // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); } lock (OdeLock) @@ -4043,8 +4028,8 @@ namespace OpenSim.Region.Physics.OdePlugin } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, - (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, - offset, thickness, wrap); + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); WaterGeom = d.CreateHeightfield(space, HeightmapData, 1); if (WaterGeom != IntPtr.Zero) @@ -4072,7 +4057,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetPosition(WaterGeom, 128, 128, 0); } } - +*/ public override void Dispose() { _worldInitialized = false; -- cgit v1.1 From 63f13b901a1a34ff7ab049ebf6c5e758d28bdb0e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Thu, 20 Aug 2015 18:15:58 +0100 Subject: ubitode a bit more friendly for megas (still disabled) --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 220 +++++------------------ 1 file changed, 49 insertions(+), 171 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 6612e97..dafd3a3 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -189,6 +189,9 @@ namespace OpenSim.Region.Physics.OdePlugin public float AvatarFriction = 0;// 0.9f * 0.5f; + // this netx dimensions are only relevant for terrain partition (mega regions) + // WorldExtents below has the simulation dimensions + // they should be identical except on mega regions private uint m_regionWidth = Constants.RegionSize; private uint m_regionHeight = Constants.RegionSize; @@ -208,11 +211,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float waterlevel = 0f; private int framecount = 0; - private int m_meshExpireCntr; - -// private IntPtr WaterGeom = IntPtr.Zero; -// private IntPtr WaterHeightmapData = IntPtr.Zero; -// private GCHandle WaterMapHandler = new GCHandle(); +// private int m_meshExpireCntr; private float avDensity = 3f; private float avMovementDivisorWalk = 1.3f; @@ -223,14 +222,9 @@ namespace OpenSim.Region.Physics.OdePlugin public float geomDefaultDensity = 10.000006836f; -// public int geomContactPointsStartthrottle = 3; -// public int geomUpdatesPerThrottledUpdate = 15; - public float bodyPIDD = 35f; public float bodyPIDG = 25; -// public int geomCrossingFailuresBeforeOutofbounds = 6; - public int bodyFramesAutoDisable = 5; private d.NearCallback nearCallback; @@ -293,7 +287,8 @@ namespace OpenSim.Region.Physics.OdePlugin // some speedup variables private int spaceGridMaxX; private int spaceGridMaxY; - private float spacesPerMeter; + private float spacesPerMeterX; + private float spacesPerMeterY; // split static geometry collision into a grid as before private IntPtr[,] staticPrimspace; @@ -423,7 +418,10 @@ namespace OpenSim.Region.Physics.OdePlugin public override void Initialise(IMesher meshmerizer, IConfigSource config, Vector3 regionExtent) { WorldExtents.X = regionExtent.X; + m_regionWidth = (uint)regionExtent.X; WorldExtents.Y = regionExtent.Y; + m_regionHeight = (uint)regionExtent.Y; + m_suportCombine = false; Initialise(meshmerizer, config); } @@ -446,14 +444,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - /* - if (region != null) - { - WorldExtents.X = region.RegionSizeX; - WorldExtents.Y = region.RegionSizeY; - } - */ - // Defaults int contactsPerCollision = 80; @@ -474,7 +464,6 @@ namespace OpenSim.Region.Physics.OdePlugin // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); -// m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations); avDensity = physicsconfig.GetFloat("av_density", avDensity); avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); @@ -482,10 +471,6 @@ namespace OpenSim.Region.Physics.OdePlugin contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); -// geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); -// geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); -// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); - geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); @@ -562,14 +547,22 @@ namespace OpenSim.Region.Physics.OdePlugin m_materialContactsData[(int)Material.light].bounce = 0.0f; - spacesPerMeter = 1 / metersInSpace; - spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); - spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter); + spacesPerMeterX = 1.0f / metersInSpace; + spacesPerMeterY = spacesPerMeterX; + spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX); + spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY); if (spaceGridMaxX > 40) + { spaceGridMaxX = 40; + spacesPerMeterX = WorldExtents.X / spaceGridMaxX; + } + if (spaceGridMaxY > 40) + { spaceGridMaxY = 40; + spacesPerMeterY = WorldExtents.Y / spaceGridMaxY; + } staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; @@ -596,7 +589,8 @@ namespace OpenSim.Region.Physics.OdePlugin staticPrimspace[i, j] = newspace; } - // let this now be real maximum values + + // let this now be index limit spaceGridMaxX--; spaceGridMaxY--; @@ -1362,18 +1356,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } } - /// - /// This is called from within simulate but outside the locked portion - /// We need to do our own locking here - /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in - /// Simulate() -- justincc). - /// - /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. - /// - /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory - /// that the space was using. - /// - /// + public void RemovePrimThreadLocked(OdePrim prim) { //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); @@ -1496,11 +1479,11 @@ namespace OpenSim.Region.Physics.OdePlugin if (pos.Y < 0) return staticPrimspaceOffRegion[2]; - x = (int)(pos.X * spacesPerMeter); + x = (int)(pos.X * spacesPerMeterX); if (x > spaceGridMaxX) return staticPrimspaceOffRegion[1]; - y = (int)(pos.Y * spacesPerMeter); + y = (int)(pos.Y * spacesPerMeterY); if (y > spaceGridMaxY) return staticPrimspaceOffRegion[3]; @@ -1694,10 +1677,7 @@ namespace OpenSim.Region.Physics.OdePlugin aprim.Move(); } } - - //if ((framecount % m_randomizeWater) == 0) - // randomizeWater(waterlevel); - + m_rayCastManager.ProcessQueuedRequests(); collision_optimized(); @@ -1896,7 +1876,6 @@ namespace OpenSim.Region.Physics.OdePlugin public float GetTerrainHeightAtXY(float x, float y) { - IntPtr heightFieldGeom = IntPtr.Zero; int offsetX = 0; int offsetY = 0; @@ -1905,16 +1884,12 @@ namespace OpenSim.Region.Physics.OdePlugin { offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - // get region map - if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) - return 0f; - } - else - { - if (!RegionTerrain.TryGetValue(Vector3.Zero , out heightFieldGeom)) - return 0f; } + // get region map + IntPtr heightFieldGeom = IntPtr.Zero; + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return 0f; if (heightFieldGeom == IntPtr.Zero) return 0f; @@ -1939,8 +1914,8 @@ namespace OpenSim.Region.Physics.OdePlugin float dx; float dy; - int regsizeX = (int)WorldExtents.X + 3; // map size see setterrain number of samples - int regsizeY = (int)WorldExtents.Y + 3; // map size see setterrain number of samples + int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples + int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples int regsize = regsizeX; if (OdeUbitLib) @@ -2040,9 +2015,6 @@ namespace OpenSim.Region.Physics.OdePlugin public Vector3 GetTerrainNormalAtXY(float x, float y) { - IntPtr heightFieldGeom = IntPtr.Zero; - Vector3 norm = new Vector3(0, 0, 1); - int offsetX = 0; int offsetY = 0; @@ -2050,16 +2022,15 @@ namespace OpenSim.Region.Physics.OdePlugin { offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - // get region map - if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) - return norm; ; - } - else - { - if (!RegionTerrain.TryGetValue(Vector3.Zero, out heightFieldGeom)) - return norm; ; } + // get region map + IntPtr heightFieldGeom = IntPtr.Zero; + Vector3 norm = new Vector3(0, 0, 1); + + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return norm; ; + if (heightFieldGeom == IntPtr.Zero) return norm; @@ -2083,8 +2054,8 @@ namespace OpenSim.Region.Physics.OdePlugin float dx; float dy; - int regsizeX = (int)WorldExtents.X + 3; // map size see setterrain number of samples - int regsizeY = (int)WorldExtents.Y + 3; // map size see setterrain number of samples + int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples + int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples int regsize = regsizeX; int xstep = 1; @@ -2197,7 +2168,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CombineTerrain(float[] heightMap, Vector3 pOffset) { - SetTerrain(heightMap, pOffset); + if(m_suportCombine) + SetTerrain(heightMap, pOffset); } public void SetTerrain(float[] heightMap, Vector3 pOffset) @@ -2215,8 +2187,8 @@ namespace OpenSim.Region.Physics.OdePlugin float[] _heightmap; - uint regionsizeX = (uint)WorldExtents.X; - uint regionsizeY = (uint)WorldExtents.Y; + uint regionsizeX = m_regionWidth; + uint regionsizeY = m_regionHeight; // map is rotated uint heightmapWidth = regionsizeY + 2; @@ -2326,7 +2298,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, pOffset.X + WorldExtents.X * 0.5f, pOffset.Y + WorldExtents.Y * 0.5f, 0); + d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); RegionTerrain.Add(pOffset, GroundGeom); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); @@ -2341,8 +2313,8 @@ namespace OpenSim.Region.Physics.OdePlugin float[] _heightmap; - uint regionsizeX = (uint)WorldExtents.X; - uint regionsizeY = (uint)WorldExtents.Y; + uint regionsizeX = m_regionWidth; + uint regionsizeY = m_regionHeight; uint heightmapWidth = regionsizeX + 2; uint heightmapHeight = regionsizeY + 2; @@ -2436,7 +2408,7 @@ namespace OpenSim.Region.Physics.OdePlugin // geom_name_map[GroundGeom] = "Terrain"; - d.GeomSetPosition(GroundGeom, pOffset.X + WorldExtents.X * 0.5f, pOffset.Y + WorldExtents.Y * 0.5f, 0); + d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); RegionTerrain.Add(pOffset, GroundGeom); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); @@ -2521,90 +2493,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SetWaterLevel(float baseheight) { waterlevel = baseheight; -// randomizeWater(waterlevel); } -/* - public void randomizeWater(float baseheight) - { - const uint heightmapWidth = Constants.RegionSize + 2; - const uint heightmapHeight = Constants.RegionSize + 2; - const uint heightmapWidthSamples = heightmapWidth + 1; - const uint heightmapHeightSamples = heightmapHeight + 1; - const float scale = 1.0f; - const float offset = 0.0f; - const int wrap = 0; - - float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples]; - - float maxheigh = float.MinValue; - float minheigh = float.MaxValue; - float val; - for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++) - { - - val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f); - _watermap[i] = val; - if (maxheigh < val) - maxheigh = val; - if (minheigh > val) - minheigh = val; - } - - float thickness = minheigh; - - lock (OdeLock) - { - if (WaterGeom != IntPtr.Zero) - { - actor_name_map.Remove(WaterGeom); - d.GeomDestroy(WaterGeom); - d.GeomHeightfieldDataDestroy(WaterHeightmapData); - WaterGeom = IntPtr.Zero; - WaterHeightmapData = IntPtr.Zero; - if(WaterMapHandler.IsAllocated) - WaterMapHandler.Free(); - } - - WaterHeightmapData = d.GeomHeightfieldDataCreate(); - - WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned); - - d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight, - (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, - offset, thickness, wrap); - d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh); - WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1); - if (WaterGeom != IntPtr.Zero) - { - d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water)); - d.GeomSetCollideBits(WaterGeom, 0); - - - PhysicsActor pa = new NullPhysicsActor(); - pa.Name = "Water"; - pa.PhysicsActorType = (int)ActorTypes.Water; - - actor_name_map[WaterGeom] = pa; -// geom_name_map[WaterGeom] = "Water"; - - d.Matrix3 R = new d.Matrix3(); - - Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); - Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); - - q1 = q1 * q2; - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); - - d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); - d.GeomSetRotation(WaterGeom, ref R); - d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0); - } - } - } -*/ public override void Dispose() { if (m_meshWorker != null) @@ -2658,19 +2548,7 @@ namespace OpenSim.Region.Physics.OdePlugin TerrainHeightFieldHeightsHandlers.Clear(); TerrainHeightFieldHeights.Clear(); -/* - if (WaterGeom != IntPtr.Zero) - { - d.GeomDestroy(WaterGeom); - WaterGeom = IntPtr.Zero; - if (WaterHeightmapData != IntPtr.Zero) - d.GeomHeightfieldDataDestroy(WaterHeightmapData); - WaterHeightmapData = IntPtr.Zero; - if (WaterMapHandler.IsAllocated) - WaterMapHandler.Free(); - } -*/ if (ContactgeomsArray != IntPtr.Zero) Marshal.FreeHGlobal(ContactgeomsArray); if (GlobalContactsArray != IntPtr.Zero) -- cgit v1.1 From 0900f9dd7fb0d586427b84261787d3db504e19f6 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 31 Aug 2015 13:06:41 +0100 Subject: fix slow moving physical objects moving without sending updates --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index ebaa50f..b13f601 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -3588,17 +3588,17 @@ namespace OpenSim.Region.Physics.OdePlugin m_rotationalVelocity.Y = vel.Y; m_rotationalVelocity.Z = vel.Z; } - } - - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; + // } - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + } if (_zeroFlag) { if (lastZeroFlag) -- cgit v1.1