From 87590491b52e8ca0211274c17a83482534f845f3 Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Thu, 4 Mar 2010 16:33:26 -0500 Subject: [PATCH] Fix Physics angular reference frame. --- .../ChOdePlugin/ODEDynamics.c_comments | 630 ------------------ .../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 diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments deleted file mode 100644 index 1060aa69c6..0000000000 --- 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 55d6945d3a..b3b09e6b05 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 8502aef768..fb6cb55946 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;