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/ChOdePlugin') 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/ChOdePlugin') 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 +++++++++++++-------------- 1 file changed, 18 insertions(+), 20 deletions(-) (limited to 'OpenSim/Region/Physics/ChOdePlugin') 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); -- 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/ChOdePlugin') 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