From 70d5b1c34cf2eb6621f383169fdee03966850762 Mon Sep 17 00:00:00 2001 From: Jeff Ames Date: Mon, 4 Jan 2010 06:10:45 +0900 Subject: Formatting cleanup. Add copyright headers. --- OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs | 440 ++++++++-------- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 562 +++++++++++---------- .../Physics/OdePlugin/ODERayCastRequestManager.cs | 2 +- OpenSim/Region/Physics/OdePlugin/OdePlugin.cs | 20 +- 4 files changed, 509 insertions(+), 515 deletions(-) (limited to 'OpenSim/Region/Physics/OdePlugin') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs index 39cdc0f..008070b 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs @@ -32,11 +32,11 @@ * 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 + * 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; @@ -53,7 +53,7 @@ namespace OpenSim.Region.Physics.OdePlugin public class ODEDynamics { public Vehicle Type - { + { get { return m_type; } } @@ -62,14 +62,14 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_body; } } - private int frcount = 0; // Used to limit dynamics debug output to - // every 100th frame + 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 @@ -82,7 +82,7 @@ namespace OpenSim.Region.Physics.OdePlugin // 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 @@ -91,47 +91,43 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_linearMotorDecayTimescale = 0; private float m_linearMotorTimescale = 0; private Vector3 m_lastLinearVelocityVector = Vector3.Zero; - // private bool m_LinearMotorSetLastFrame = false; + // 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 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 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 + //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_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 + // 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) { @@ -166,13 +162,13 @@ namespace OpenSim.Region.Physics.OdePlugin // 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; +// if (pValue < 0f) pValue = 0f; +// if (pValue > 1f) pValue = 1f; // m_VhoverEfficiency = pValue; // break; case Vehicle.HOVER_HEIGHT: @@ -199,7 +195,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorTimescale = pValue; break; case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: - if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable + if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable if (pValue > 1.0f) pValue = 1.0f; m_verticalAttractionEfficiency = pValue; break; @@ -207,8 +203,8 @@ namespace OpenSim.Region.Physics.OdePlugin 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 + + // 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); @@ -229,7 +225,6 @@ namespace OpenSim.Region.Physics.OdePlugin break; } - }//end ProcessFloatVehicleParam internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) @@ -242,12 +237,12 @@ namespace OpenSim.Region.Physics.OdePlugin 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; m_angularMotorApply = 10; break; case Vehicle.LINEAR_FRICTION_TIMESCALE: @@ -261,7 +256,6 @@ namespace OpenSim.Region.Physics.OdePlugin // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); break; } - }//end ProcessVectorVehicleParam internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) @@ -272,12 +266,11 @@ namespace OpenSim.Region.Physics.OdePlugin // m_referenceFrame = pValue; break; } - }//end ProcessRotationVehicleParam internal void ProcessTypeChange(Vehicle pType) { - // Set Defaults For Type + // Set Defaults For Type m_type = pType; switch (pType) { @@ -357,8 +350,8 @@ namespace OpenSim.Region.Physics.OdePlugin // 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; @@ -432,24 +425,25 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) return; - frcount++; // used to limit debug comment output + frcount++; // used to limit debug comment output if (frcount > 100) frcount = 0; - MoveLinear(pTimestep, pParentScene); + 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 (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + 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? - + 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)) @@ -458,7 +452,7 @@ namespace OpenSim.Region.Physics.OdePlugin 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); @@ -466,194 +460,192 @@ namespace OpenSim.Region.Physics.OdePlugin //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 + { // 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 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. + // 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; - // 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); + 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); + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); - // apply friction + // 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 - */ - - // 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 - + /* + 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 + */ + + // 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; // + bank + deflection - - if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) + Vector3 vertattr = Vector3.Zero; + + if (m_verticalAttractionTimescale < 300) { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - } - else - { - m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. - } - - // apply friction + 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; // + 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 - } + 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/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index 3eb3b28..973aa84 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -1,5 +1,7 @@ -/* Copyright (c) Contributors, http://opensimulator.org/ +/* + * 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 @@ -93,7 +95,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; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. // private float m_tensor = 5f; private int body_autodisable_frames = 20; @@ -294,7 +296,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintselected = value; m_isSelected = value; } - if(m_isSelected) disableBodySoft(); + if (m_isSelected) disableBodySoft(); } } @@ -302,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin { prev_geom = 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) { d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); @@ -314,7 +316,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); } } @@ -331,7 +333,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodyEnable(Body); if (m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Enable(Body, _parent_scene); + m_vehicle.Enable(Body, _parent_scene); } m_disabled = false; @@ -376,7 +378,7 @@ namespace OpenSim.Region.Physics.OdePlugin 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; @@ -872,7 +874,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void ProcessTaints(float timestep) { -//Console.WriteLine("ProcessTaints for " + m_primName ); +//Console.WriteLine("ProcessTaints for " + m_primName); if (m_taintadd) { changeadd(timestep); @@ -880,24 +882,24 @@ 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... - { - 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 (!_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); - } + //Just rotate the prim + rotate(timestep); + } } // @@ -1006,7 +1008,7 @@ namespace OpenSim.Region.Physics.OdePlugin // destroy link else if (_parent != null && m_taintparent == null) { -//Console.WriteLine(" changelink B"); +//Console.WriteLine(" changelink B"); if (_parent is OdePrim) { @@ -1033,7 +1035,7 @@ namespace OpenSim.Region.Physics.OdePlugin // prim is the child public void ParentPrim(OdePrim prim) { -//Console.WriteLine("ParentPrim " + m_primName); +//Console.WriteLine("ParentPrim " + m_primName); if (this.m_localID != prim.m_localID) { if (Body == IntPtr.Zero) @@ -1047,7 +1049,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (!childrenPrim.Contains(prim)) { -//Console.WriteLine("childrenPrim.Add " + prim); +//Console.WriteLine("childrenPrim.Add " + prim); childrenPrim.Add(prim); foreach (OdePrim prm in childrenPrim) @@ -1080,7 +1082,7 @@ 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); +//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); @@ -1128,7 +1130,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); +//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); @@ -1203,7 +1205,7 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdePrim prm in childrenPrim) { -//Console.WriteLine("ChildSetGeom calls ParentPrim"); +//Console.WriteLine("ChildSetGeom calls ParentPrim"); ParentPrim(prm); } } @@ -1230,7 +1232,7 @@ namespace OpenSim.Region.Physics.OdePlugin lock (childrenPrim) { - //Console.WriteLine("childrenPrim.Remove " + odePrim); + //Console.WriteLine("childrenPrim.Remove " + odePrim); childrenPrim.Remove(odePrim); } @@ -1248,7 +1250,7 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdePrim prm in childrenPrim) { -//Console.WriteLine("ChildDelink calls ParentPrim"); +//Console.WriteLine("ChildDelink calls ParentPrim"); ParentPrim(prm); } } @@ -1350,7 +1352,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) { -//Console.WriteLine("CreateGeom:"); +//Console.WriteLine("CreateGeom:"); if (_mesh != null) { setMesh(_parent_scene, _mesh); @@ -1381,7 +1383,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 2"); +//Console.WriteLine(" CreateGeom 2"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1397,7 +1399,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 3"); +//Console.WriteLine(" CreateGeom 3"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1414,7 +1416,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 4"); +//Console.WriteLine(" CreateGeom 4"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1451,7 +1453,7 @@ namespace OpenSim.Region.Physics.OdePlugin lock (_parent_scene.OdeLock) { -//Console.WriteLine("changeadd 1"); +//Console.WriteLine("changeadd 1"); CreateGeom(m_targetSpace, _mesh); if (prim_geom != IntPtr.Zero) @@ -1508,7 +1510,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(" JointCreateFixed"); m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); d.JointAttach(m_linkJoint, Body, odParent.Body); d.JointSetFixed(m_linkJoint); @@ -1562,244 +1564,244 @@ Console.WriteLine(" JointCreateFixed"); float fz = 0; - 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 " + 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)) - { - 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; + 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 " + 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)) + { + 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 " + 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() - - // 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 = + + //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 " + 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() + + // 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; - } + (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() - // 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 = + // 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 = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } - - 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; + (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 = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } + + 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); - } - } + 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 + { // is not physical, or is not a body or is selected // _zeroPosition = d.BodyGetPosition(Body); return; -//Console.WriteLine("Nothing " + m_primName); +//Console.WriteLine("Nothing " + m_primName); } } @@ -1815,18 +1817,18 @@ Console.WriteLine(" 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); - if (m_isphysical) - { + 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); + createAMotor(m_angularlock); + } + } + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); } resetCollisionAccounting(); @@ -1890,7 +1892,7 @@ Console.WriteLine(" JointCreateFixed"); m_log.Error("[PHYSICS]: PrimGeom dead"); } } -//Console.WriteLine("changePhysicsStatus for " + m_primName ); +//Console.WriteLine("changePhysicsStatus for " + m_primName); changeadd(2f); } if (childPrim) @@ -1976,7 +1978,7 @@ Console.WriteLine(" JointCreateFixed"); else { _mesh = null; -//Console.WriteLine("changesize 2"); +//Console.WriteLine("changesize 2"); CreateGeom(m_targetSpace, _mesh); } @@ -2083,7 +2085,7 @@ Console.WriteLine(" JointCreateFixed"); else { _mesh = null; -//Console.WriteLine("changeshape"); +//Console.WriteLine("changeshape"); CreateGeom(m_targetSpace, null); } @@ -2454,7 +2456,7 @@ Console.WriteLine(" JointCreateFixed"); if (QuaternionIsFinite(value)) { _orientation = value; - } + } else m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); @@ -2675,8 +2677,8 @@ Console.WriteLine(" JointCreateFixed"); //outofBounds = true; } - //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); -//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); + //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) @@ -2684,7 +2686,7 @@ Console.WriteLine(" JointCreateFixed"); && (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"); +//Console.WriteLine("ZFT 2"); m_throttleUpdates = false; } else diff --git a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs index 7314107..ba77dae 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs @@ -110,7 +110,7 @@ namespace OpenSim.Region.Physics.OdePlugin 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 + RayCast(reqs[i]); // if there isn't anyone to send results } /* foreach (ODERayCastRequest req in m_PendingRequests) diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs index 0384d6e..7984bd9 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs @@ -2152,7 +2152,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// public void RemovePrimThreadLocked(OdePrim prim) { -//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); +//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); lock (prim) { remCollisionEventReporting(prim); @@ -2603,12 +2603,12 @@ namespace OpenSim.Region.Physics.OdePlugin 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 - } - } + { +//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) @@ -2736,12 +2736,12 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prim.m_taintremove) { - //Console.WriteLine("Simulate calls RemovePrimThreadLocked"); + //Console.WriteLine("Simulate calls RemovePrimThreadLocked"); RemovePrimThreadLocked(prim); } else { - //Console.WriteLine("Simulate calls ProcessTaints"); + //Console.WriteLine("Simulate calls ProcessTaints"); prim.ProcessTaints(timeStep); } processedtaints = true; @@ -2937,7 +2937,7 @@ namespace OpenSim.Region.Physics.OdePlugin } if (processedtaints) -//Console.WriteLine("Simulate calls Clear of _taintedPrim list"); +//Console.WriteLine("Simulate calls Clear of _taintedPrim list"); _taintedPrimH.Clear(); _taintedPrimL.Clear(); } -- cgit v1.1