OpenSim-Test/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs

/*
 * 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_<other> is to
 * switch between 'VEHICLE' parameter use and general dynamics
 * settings use.
 */

// Extensive change Ubit 2012

using System;
using OpenMetaverse;
using OpenSim.Region.PhysicsModules.SharedBase;

namespace OpenSim.Region.PhysicsModule.ubOde
{
    public class ODEDynamics
    {
        public Vehicle Type
        {
            get { return m_type; }
        }

        private OdePrim rootPrim;
        private ODEScene _pParentScene;

        // Vehicle properties
        // WARNING this are working copies for internel use
        // their values may not be the corresponding parameter

        private Quaternion m_referenceFrame = Quaternion.Identity;      // Axis modifier
        private Quaternion m_RollreferenceFrame = Quaternion.Identity;  // what hell is this ?

        private Vehicle m_type = Vehicle.TYPE_NONE;                     // If a 'VEHICLE', and what kind

        private VehicleFlag m_flags = (VehicleFlag) 0;                  // Boolean settings:
                                                                        // HOVER_TERRAIN_ONLY
                                                                        // HOVER_GLOBAL_HEIGHT
                                                                        // NO_DEFLECTION_UP
                                                                        // HOVER_WATER_ONLY
                                                                        // HOVER_UP_ONLY
                                                                        // LIMIT_MOTOR_UP
                                                                        // LIMIT_ROLL_ONLY
        private Vector3 m_BlockingEndPoint = Vector3.Zero;              // not sl

        // Linear properties
        private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
        private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
        private float m_linearMotorDecayTimescale = 120;
        private float m_linearMotorTimescale = 1000;
        private Vector3 m_linearMotorOffset = Vector3.Zero;

        //Angular properties
        private Vector3 m_angularMotorDirection = Vector3.Zero;         // angular velocity requested by LSL motor
        private float m_angularMotorTimescale = 1000;                      // motor angular velocity ramp up rate
        private float m_angularMotorDecayTimescale = 120;                 // motor angular velocity decay rate
        private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);      // body angular velocity  decay rate

        //Deflection properties
        private float m_angularDeflectionEfficiency = 0;
        private float m_angularDeflectionTimescale = 1000;
        private float m_linearDeflectionEfficiency = 0;
        private float m_linearDeflectionTimescale = 1000;

        //Banking properties
        private float m_bankingEfficiency = 0;
        private float m_bankingMix = 0;
        private float m_bankingTimescale = 1000;

        //Hover and Buoyancy properties
        private float m_VhoverHeight = 0f;
        private float m_VhoverEfficiency = 0f;
        private float m_VhoverTimescale = 1000f;
        private float m_VehicleBuoyancy = 0f;           //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
                    // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
                    // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
                    // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.

        //Attractor properties
        private float m_verticalAttractionEfficiency = 1.0f;        // damped
        private float m_verticalAttractionTimescale = 1000f;        // Timescale > 300  means no vert attractor.


        // auxiliar
        private float m_lmEfect = 0f;                                            // current linear motor eficiency
        private float m_lmDecay = 0f;                                            // current linear decay

        private float m_amEfect = 0;                                            // current angular motor eficiency
        private float m_amDecay = 0f;                                            // current linear decay

        private float m_ffactor = 1.0f;

        private float m_timestep = 0.02f;
        private float m_invtimestep = 50;


        float m_ampwr;
        float m_amdampX;
        float m_amdampY;
        float m_amdampZ;

        float m_gravmod;

        public float FrictionFactor
        {
            get
            {
                return m_ffactor;
            }
        }

        public float GravMod
        {
            set
            {
                m_gravmod = value;
            }
        }


        public ODEDynamics(OdePrim rootp)
        {
            rootPrim = rootp;
            _pParentScene = rootPrim._parent_scene;
            m_timestep = _pParentScene.ODE_STEPSIZE;
            m_invtimestep = 1.0f / m_timestep;
            m_gravmod = rootPrim.GravModifier;
        }

        public void DoSetVehicle(VehicleData vd)
        {
            m_type = vd.m_type;
            m_flags = vd.m_flags;


            // Linear properties
            m_linearMotorDirection = vd.m_linearMotorDirection;

            m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
            if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
            if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
            if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;

            m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
            if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
            m_linearMotorDecayTimescale += 0.2f;
            m_linearMotorDecayTimescale *= m_invtimestep;

            m_linearMotorTimescale = vd.m_linearMotorTimescale;
            if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;

            m_linearMotorOffset = vd.m_linearMotorOffset;

            //Angular properties
            m_angularMotorDirection = vd.m_angularMotorDirection;
            m_angularMotorTimescale = vd.m_angularMotorTimescale;
            if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;

            m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
            if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
            m_angularMotorDecayTimescale *= m_invtimestep;

            m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
            if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
            if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
            if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;

            //Deflection properties
            m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
            m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
            if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;

            m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
            m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
            if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;

            //Banking properties
            m_bankingEfficiency = vd.m_bankingEfficiency;
            m_bankingMix = vd.m_bankingMix;
            m_bankingTimescale = vd.m_bankingTimescale;
            if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;

            //Hover and Buoyancy properties
            m_VhoverHeight = vd.m_VhoverHeight;
            m_VhoverEfficiency = vd.m_VhoverEfficiency;
            m_VhoverTimescale = vd.m_VhoverTimescale;
            if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;

            m_VehicleBuoyancy = vd.m_VehicleBuoyancy;

            //Attractor properties
            m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
            m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
            if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;

            // Axis
            m_referenceFrame = vd.m_referenceFrame;

            m_lmEfect = 0;
            m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
            m_amEfect = 0;
            m_ffactor = 1.0f;
        }

        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
        {
            float len;
            if(float.IsNaN(pValue) || float.IsInfinity(pValue))
                return;

            switch (pParam)
            {
                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
                    if (pValue < 0f) pValue = 0f;
                    if (pValue > 1f) pValue = 1f;
                    m_angularDeflectionEfficiency = pValue;
                    break;
                case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_angularDeflectionTimescale = pValue;
                    break;
                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    else if (pValue > 120) pValue = 120;
                    m_angularMotorDecayTimescale = pValue * m_invtimestep;
                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
                    break;
                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_angularMotorTimescale = pValue;
                    break;
                case Vehicle.BANKING_EFFICIENCY:
                    if (pValue < -1f) pValue = -1f;
                    if (pValue > 1f) pValue = 1f;
                    m_bankingEfficiency = pValue;
                    break;
                case Vehicle.BANKING_MIX:
                    if (pValue < 0f) pValue = 0f;
                    if (pValue > 1f) pValue = 1f;
                    m_bankingMix = pValue;
                    break;
                case Vehicle.BANKING_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_bankingTimescale = pValue;
                    break;
                case Vehicle.BUOYANCY:
                    if (pValue < -1f) pValue = -1f;
                    if (pValue > 1f) pValue = 1f;
                    m_VehicleBuoyancy = pValue;
                    break;
                case Vehicle.HOVER_EFFICIENCY:
                    if (pValue < 0f) pValue = 0f;
                    if (pValue > 1f) pValue = 1f;
                    m_VhoverEfficiency = pValue;
                    break;
                case Vehicle.HOVER_HEIGHT:
                    m_VhoverHeight = pValue;
                    break;
                case Vehicle.HOVER_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_VhoverTimescale = pValue;
                    break;
                case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
                    if (pValue < 0f) pValue = 0f;
                    if (pValue > 1f) pValue = 1f;
                    m_linearDeflectionEfficiency = pValue;
                    break;
                case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_linearDeflectionTimescale = pValue;
                    break;
                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    else if (pValue > 120) pValue = 120;
                    m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
                    m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
                    break;
                case Vehicle.LINEAR_MOTOR_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_linearMotorTimescale = pValue;
                    break;
                case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
                    if (pValue < 0f) pValue = 0f;
                    if (pValue > 1f) pValue = 1f;
                    m_verticalAttractionEfficiency = pValue;
                    break;
                case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_verticalAttractionTimescale = pValue;
                    break;

                // These are vector properties but the engine lets you use a single float value to
                // set all of the components to the same value
                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
                    break;
                case Vehicle.ANGULAR_MOTOR_DIRECTION:
                    m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
                    len = m_angularMotorDirection.Length();
                    if (len > 12.566f)
                        m_angularMotorDirection *= (12.566f / len);

                    m_amEfect = 1.0f ; // turn it on
                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;

                    if (rootPrim.Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(rootPrim.Body)
                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
                        SafeNativeMethods.BodyEnable(rootPrim.Body);

                    break;
                case Vehicle.LINEAR_FRICTION_TIMESCALE:
                    if (pValue < m_timestep) pValue = m_timestep;
                    m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
                    break;
                case Vehicle.LINEAR_MOTOR_DIRECTION:
                    m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
                    len = m_linearMotorDirection.Length();
                    if (len > 100.0f)
                        m_linearMotorDirection *= (100.0f / len);

                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
                    m_lmEfect = 1.0f; // turn it on

                    m_ffactor = 0.0f;
                    if (rootPrim.Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(rootPrim.Body)
                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
                        SafeNativeMethods.BodyEnable(rootPrim.Body);
                    break;
                case Vehicle.LINEAR_MOTOR_OFFSET:
                    m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
                    len = m_linearMotorOffset.Length();
                    if (len > 100.0f)
                        m_linearMotorOffset *= (100.0f / len);
                    break;
            }
        }//end ProcessFloatVehicleParam

        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
        {
            float len;
            if(!pValue.IsFinite())
                return;

            switch (pParam)
            {
                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
                    if (pValue.X < m_timestep) pValue.X = m_timestep;
                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;

                    m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
                    break;
                case Vehicle.ANGULAR_MOTOR_DIRECTION:
                    m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
                    // Limit requested angular speed to 2 rps= 4 pi rads/sec
                    len = m_angularMotorDirection.Length();
                    if (len > 12.566f)
                        m_angularMotorDirection *= (12.566f / len);

                    m_amEfect = 1.0f; // turn it on
                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;

                    if (rootPrim.Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(rootPrim.Body)
                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
                        SafeNativeMethods.BodyEnable(rootPrim.Body);
                    break;
                case Vehicle.LINEAR_FRICTION_TIMESCALE:
                    if (pValue.X < m_timestep) pValue.X = m_timestep;
                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
                    break;
                case Vehicle.LINEAR_MOTOR_DIRECTION:
                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
                    len = m_linearMotorDirection.Length();
                    if (len > 100.0f)
                        m_linearMotorDirection *= (100.0f / len);

                    m_lmEfect = 1.0f; // turn it on
                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;

                    m_ffactor = 0.0f;
                    if (rootPrim.Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(rootPrim.Body)
                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
                        SafeNativeMethods.BodyEnable(rootPrim.Body);
                    break;
                case Vehicle.LINEAR_MOTOR_OFFSET:
                    m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
                    len = m_linearMotorOffset.Length();
                    if (len > 100.0f)
                        m_linearMotorOffset *= (100.0f / len);
                    break;
                case Vehicle.BLOCK_EXIT:
                    m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
                    break;
            }
        }//end ProcessVectorVehicleParam

        internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
        {
            switch (pParam)
            {
                case Vehicle.REFERENCE_FRAME:
                    //                    m_referenceFrame = Quaternion.Inverse(pValue);
                    m_referenceFrame = pValue;
                    break;
                case Vehicle.ROLL_FRAME:
                    m_RollreferenceFrame = pValue;
                    break;
            }
        }//end ProcessRotationVehicleParam

        internal void ProcessVehicleFlags(int pParam, bool remove)
        {
            if (remove)
            {
                m_flags &= ~((VehicleFlag)pParam);
            }
            else
            {
                m_flags |= (VehicleFlag)pParam;
            }
        }//end ProcessVehicleFlags

        internal void ProcessTypeChange(Vehicle pType)
        {
            m_lmEfect = 0;

            m_amEfect = 0;
            m_ffactor = 1f;

            m_linearMotorDirection = Vector3.Zero;
            m_angularMotorDirection = Vector3.Zero;

            m_BlockingEndPoint = Vector3.Zero;
            m_RollreferenceFrame = Quaternion.Identity;
            m_linearMotorOffset = Vector3.Zero;

            m_referenceFrame = Quaternion.Identity;

            // Set Defaults For Type
            m_type = pType;
            switch (pType)
            {
                case Vehicle.TYPE_NONE:
                    m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                    m_linearMotorTimescale = 1000;
                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
                    m_angularMotorTimescale = 1000;
                    m_angularMotorDecayTimescale = 1000  * m_invtimestep;
                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 1;
                    m_VhoverTimescale = 1000;
                    m_VehicleBuoyancy = 0;
                    m_linearDeflectionEfficiency = 0;
                    m_linearDeflectionTimescale = 1000;
                    m_angularDeflectionEfficiency = 0;
                    m_angularDeflectionTimescale = 1000;
                    m_bankingEfficiency = 0;
                    m_bankingMix = 1;
                    m_bankingTimescale = 1000;
                    m_verticalAttractionEfficiency = 0;
                    m_verticalAttractionTimescale = 1000;

                    m_flags = (VehicleFlag)0;
                    break;

                case Vehicle.TYPE_SLED:
                    m_linearFrictionTimescale = new Vector3(30, 1, 1000);
                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                    m_linearMotorTimescale = 1000;
                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
                    m_angularMotorTimescale = 1000;
                    m_angularMotorDecayTimescale = 120 * m_invtimestep;
                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 1;
                    m_VhoverTimescale = 10;
                    m_VehicleBuoyancy = 0;
                    m_linearDeflectionEfficiency = 1;
                    m_linearDeflectionTimescale = 1;
                    m_angularDeflectionEfficiency = 0;
                    m_angularDeflectionTimescale = 10;
                    m_verticalAttractionEfficiency = 1;
                    m_verticalAttractionTimescale = 1000;
                    m_bankingEfficiency = 0;
                    m_bankingMix = 1;
                    m_bankingTimescale = 10;
                    m_flags &=
                         ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                           VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
                        VehicleFlag.LIMIT_ROLL_ONLY |
                        VehicleFlag.LIMIT_MOTOR_UP);
                    break;

                case Vehicle.TYPE_CAR:
                    m_linearFrictionTimescale = new Vector3(100, 2, 1000);
                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                    m_linearMotorTimescale = 1;
                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
                    m_angularMotorTimescale = 1;
                    m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 0;
                    m_VhoverTimescale = 1000;
                    m_VehicleBuoyancy = 0;
                    m_linearDeflectionEfficiency = 1;
                    m_linearDeflectionTimescale = 2;
                    m_angularDeflectionEfficiency = 0;
                    m_angularDeflectionTimescale = 10;
                    m_verticalAttractionEfficiency = 1f;
                    m_verticalAttractionTimescale = 10f;
                    m_bankingEfficiency = -0.2f;
                    m_bankingMix = 1;
                    m_bankingTimescale = 1;
                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
                                VehicleFlag.HOVER_TERRAIN_ONLY |
                                VehicleFlag.HOVER_GLOBAL_HEIGHT);
                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
                                VehicleFlag.LIMIT_ROLL_ONLY |
                                VehicleFlag.LIMIT_MOTOR_UP |
                                VehicleFlag.HOVER_UP_ONLY);
                    break;
                case Vehicle.TYPE_BOAT:
                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
                    m_linearMotorTimescale = 5;
                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
                    m_angularMotorTimescale = 4;
                    m_angularMotorDecayTimescale = 4 * m_invtimestep;
                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 0.5f;
                    m_VhoverTimescale = 2;
                    m_VehicleBuoyancy = 1;
                    m_linearDeflectionEfficiency = 0.5f;
                    m_linearDeflectionTimescale = 3;
                    m_angularDeflectionEfficiency = 0.5f;
                    m_angularDeflectionTimescale = 5;
                    m_verticalAttractionEfficiency = 0.5f;
                    m_verticalAttractionTimescale = 5f;
                    m_bankingEfficiency = -0.3f;
                    m_bankingMix = 0.8f;
                    m_bankingTimescale = 1;
                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
                            VehicleFlag.HOVER_GLOBAL_HEIGHT |
                            VehicleFlag.HOVER_UP_ONLY); // |
//                            VehicleFlag.LIMIT_ROLL_ONLY);
                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
                                VehicleFlag.LIMIT_MOTOR_UP |
                                VehicleFlag.HOVER_UP_ONLY |  // new sl
                                VehicleFlag.HOVER_WATER_ONLY);
                    break;

                case Vehicle.TYPE_AIRPLANE:
                    m_linearFrictionTimescale = new Vector3(200, 10, 5);
                    m_angularFrictionTimescale = new Vector3(20, 20, 20);
                    m_linearMotorTimescale = 2;
                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
                    m_angularMotorTimescale = 4;
                    m_angularMotorDecayTimescale = 8 * m_invtimestep;
                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 0.5f;
                    m_VhoverTimescale = 1000;
                    m_VehicleBuoyancy = 0;
                    m_linearDeflectionEfficiency = 0.5f;
                    m_linearDeflectionTimescale = 0.5f;
                    m_angularDeflectionEfficiency = 1;
                    m_angularDeflectionTimescale = 2;
                    m_verticalAttractionEfficiency = 0.9f;
                    m_verticalAttractionTimescale = 2f;
                    m_bankingEfficiency = 1;
                    m_bankingMix = 0.7f;
                    m_bankingTimescale = 2;
                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
                        VehicleFlag.HOVER_TERRAIN_ONLY |
                        VehicleFlag.HOVER_GLOBAL_HEIGHT |
                        VehicleFlag.HOVER_UP_ONLY |
                        VehicleFlag.NO_DEFLECTION_UP |
                        VehicleFlag.LIMIT_MOTOR_UP);
                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
                    break;

                case Vehicle.TYPE_BALLOON:
                    m_linearFrictionTimescale = new Vector3(5, 5, 5);
                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
                    m_linearMotorTimescale = 5;
                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
                    m_angularMotorTimescale = 6;
                    m_angularMotorDecayTimescale = 10 * m_invtimestep;
                    m_VhoverHeight = 5;
                    m_VhoverEfficiency = 0.8f;
                    m_VhoverTimescale = 10;
                    m_VehicleBuoyancy = 1;
                    m_linearDeflectionEfficiency = 0;
                    m_linearDeflectionTimescale = 5 * m_invtimestep;
                    m_angularDeflectionEfficiency = 0;
                    m_angularDeflectionTimescale = 5;
                    m_verticalAttractionEfficiency = 1f;
                    m_verticalAttractionTimescale = 1000f;
                    m_bankingEfficiency = 0;
                    m_bankingMix = 0.7f;
                    m_bankingTimescale = 5;
                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
                        VehicleFlag.HOVER_TERRAIN_ONLY |
                        VehicleFlag.HOVER_UP_ONLY |
                        VehicleFlag.NO_DEFLECTION_UP |
                        VehicleFlag.LIMIT_MOTOR_UP  | //);
                        VehicleFlag.LIMIT_ROLL_ONLY | // new sl
                        VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl

//                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
//                        VehicleFlag.HOVER_GLOBAL_HEIGHT);
                    break;

            }
            // disable mouse steering
            m_flags &= ~(VehicleFlag.MOUSELOOK_STEER |
                         VehicleFlag.MOUSELOOK_BANK  |
                         VehicleFlag.CAMERA_DECOUPLED);

            m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
            m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;

        }//end SetDefaultsForType

        internal void Stop()
        {
            m_lmEfect = 0;
            m_lmDecay = 0f;
            m_amEfect = 0;
            m_amDecay = 0;
            m_ffactor = 1f;
        }

        public static Vector3 Xrot(Quaternion rot)
        {
            Vector3 vec;
            rot.Normalize(); // just in case
            vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
            vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
            vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
            return vec;
        }

        public static Vector3 Zrot(Quaternion rot)
        {
            Vector3 vec;
            rot.Normalize(); // just in case
            vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
            vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
            vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;

            return vec;
        }

        private const float pi = (float)Math.PI;
        private const float halfpi = 0.5f * (float)Math.PI;
        private const float twopi = 2.0f * pi;

        public static Vector3 ubRot2Euler(Quaternion rot)
        {
            // returns roll in X
            //         pitch in Y
            //         yaw in Z
            Vector3 vec;

            // assuming rot is normalised
            // rot.Normalize();

            float zX = rot.X * rot.Z + rot.Y * rot.W;

            if (zX < -0.49999f)
            {
                vec.X = 0;
                vec.Y = -halfpi;
                vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
            }
            else if (zX > 0.49999f)
            {
                vec.X = 0;
                vec.Y = halfpi;
                vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
            }
            else
            {
                vec.Y = (float)Math.Asin(2 * zX);

                float sqw = rot.W * rot.W;

                float minuszY = rot.X * rot.W - rot.Y * rot.Z;
                float zZ = rot.Z * rot.Z + sqw - 0.5f;

                vec.X = (float)Math.Atan2(minuszY, zZ);

                float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
                float yY = rot.X * rot.X + sqw - 0.5f;
                vec.Z = (float)Math.Atan2(yX, yY);
            }
            return vec;
        }

        public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
        {
            // assuming rot is normalised
            // rot.Normalize();

            float zX = rot.X * rot.Z + rot.Y * rot.W;

            if (zX < -0.49999f)
            {
                roll = 0;
                pitch = -halfpi;
            }
            else if (zX > 0.49999f)
            {
                roll = 0;
                pitch = halfpi;
            }
            else
            {
                pitch = (float)Math.Asin(2 * zX);

                float minuszY = rot.X * rot.W - rot.Y * rot.Z;
                float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;

                roll = (float)Math.Atan2(minuszY, zZ);
            }
            return ;
        }

        internal void Step()
        {
            IntPtr Body = rootPrim.Body;

            SafeNativeMethods.Mass dmass;
            SafeNativeMethods.BodyGetMass(Body, out dmass);

            SafeNativeMethods.Quaternion rot = SafeNativeMethods.BodyGetQuaternion(Body);
            Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
            Quaternion rotq = objrotq;    // rotq = rotation of object
            rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
            Quaternion irotq = Quaternion.Inverse(rotq);

            SafeNativeMethods.Vector3 dvtmp;
            Vector3 tmpV;
            Vector3 curVel; // velocity in world
            Vector3 curAngVel; // angular velocity in world
            Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
            Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
            SafeNativeMethods.Vector3 dtorque = new SafeNativeMethods.Vector3();

            dvtmp = SafeNativeMethods.BodyGetLinearVel(Body);
            curVel.X = dvtmp.X;
            curVel.Y = dvtmp.Y;
            curVel.Z = dvtmp.Z;
            Vector3 curLocalVel = curVel * irotq; // current velocity in  local

            dvtmp = SafeNativeMethods.BodyGetAngularVel(Body);
            curAngVel.X = dvtmp.X;
            curAngVel.Y = dvtmp.Y;
            curAngVel.Z = dvtmp.Z;
            Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in  local

            float ldampZ = 0;

            bool mousemode = false;
            bool mousemodebank = false;

            float bankingEfficiency;
            float verticalAttractionTimescale = m_verticalAttractionTimescale;

            if((m_flags & (VehicleFlag.MOUSELOOK_STEER | VehicleFlag.MOUSELOOK_BANK)) != 0 )
            {
                mousemode = true;
                mousemodebank = (m_flags & VehicleFlag.MOUSELOOK_BANK) != 0;
                if(mousemodebank)
                {
                    bankingEfficiency = m_bankingEfficiency;
                    if(verticalAttractionTimescale < 149.9)
                        verticalAttractionTimescale *= 2.0f; // reduce current instability
                }
                else
                    bankingEfficiency = 0;
            }
            else
                bankingEfficiency = m_bankingEfficiency;

            // linear motor
            if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
            {
                tmpV = m_linearMotorDirection - curLocalVel; // velocity error
                tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
                tmpV *= rotq; // to world

                if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
                    tmpV.Z = 0;

                if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
                {
                    // have offset, do it now
                    tmpV *= dmass.mass;
                    SafeNativeMethods.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
                }
                else
                {
                    force.X += tmpV.X;
                    force.Y += tmpV.Y;
                    force.Z += tmpV.Z;
                }

                m_lmEfect *= m_lmDecay;
//                m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
                m_ffactor = 0.0f;
            }
            else
            {
                m_lmEfect = 0;
                m_ffactor = 1f;
            }

            // hover
            if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
            {
                //                d.Vector3 pos = d.BodyGetPosition(Body);
                SafeNativeMethods.Vector3 pos = SafeNativeMethods.GeomGetPosition(rootPrim.prim_geom);
                pos.Z -= 0.21f; // minor offset that seems to be always there in sl

                float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
                float perr;

                // default to global but don't go underground
                perr = m_VhoverHeight - pos.Z;

                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
                {
                    if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                    {
                        perr += _pParentScene.GetWaterLevel();
                    }
                    else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                    {
                        perr += t;
                    }
                    else
                    {
                        float w = _pParentScene.GetWaterLevel();
                        if (t > w)
                            perr += t;
                        else
                            perr += w;
                    }
                }
                else if (t > m_VhoverHeight)
                        perr = t - pos.Z; ;

                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
                {
                    ldampZ = m_VhoverEfficiency * m_invtimestep;

                    perr *= (1.0f + ldampZ) / m_VhoverTimescale;

                    //                    force.Z += perr - curVel.Z * tmp;
                    force.Z += perr;
                    ldampZ *= -curVel.Z;

                    force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
                }
                else // no buoyancy
                    force.Z += _pParentScene.gravityz;
            }
            else
            {
                // default gravity and Buoyancy
                force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
            }

            // linear deflection
            if (m_linearDeflectionEfficiency > 0)
            {
                float len = curVel.Length();
                if (len > 0.01) // if moving
                {
                    Vector3 atAxis;
                    atAxis = Xrot(rotq); // where are we pointing to
                    atAxis *= len; // make it same size as world velocity vector

                    tmpV = -atAxis; // oposite direction
                    atAxis -= curVel; // error to one direction
                    len = atAxis.LengthSquared();

                    tmpV -= curVel; // error to oposite
                    float lens = tmpV.LengthSquared();

                    if (len > 0.01 || lens > 0.01) // do nothing if close enougth
                    {
                        if (len < lens)
                            tmpV = atAxis;

                        tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
                        force.X += tmpV.X;
                        force.Y += tmpV.Y;
                        if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
                            force.Z += tmpV.Z;
                    }
                }
            }

            // linear friction/damping
            if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
            {
                tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
                tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
                tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
                tmpV *= rotq; // to world

                if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
                    tmpV.Z = ldampZ;
                force.X += tmpV.X;
                force.Y += tmpV.Y;
                force.Z += tmpV.Z;
            }

            // vertical atractor
            if (verticalAttractionTimescale < 300)
            {
                float roll;
                float pitch;

                float ftmp = m_invtimestep / verticalAttractionTimescale / verticalAttractionTimescale;

                float ftmp2;
                ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
                m_amdampX = ftmp2;

                m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;

                GetRollPitch(irotq, out roll, out pitch);

                if (roll > halfpi)
                    roll = pi - roll;
                else if (roll < -halfpi)
                    roll = -pi - roll;

                float effroll = pitch / halfpi;
                effroll *= effroll;
                effroll = 1 - effroll;
                effroll *= roll;

                torque.X += effroll * ftmp;

                if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
                {
                    float effpitch = roll / halfpi;
                    effpitch *= effpitch;
                    effpitch = 1 - effpitch;
                    effpitch *= pitch;

                    torque.Y += effpitch * ftmp;
                }

                if (bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
                {

                    float broll = effroll;
                    /*
                                        if (broll > halfpi)
                                            broll = pi - broll;
                                        else if (broll < -halfpi)
                                            broll = -pi - broll;
                    */
                    broll *= m_bankingEfficiency;
                    if (m_bankingMix != 0)
                    {
                        float vfact = Math.Abs(curLocalVel.X) / 10.0f;
                        if (vfact > 1.0f) vfact = 1.0f;

                        if (curLocalVel.X >= 0)
                            broll *= (1 + (vfact - 1) * m_bankingMix);
                        else
                            broll *= -(1 + (vfact - 1) * m_bankingMix);
                    }
                    // make z rot be in world Z not local as seems to be in sl

                    broll = broll / m_bankingTimescale;


                    tmpV = Zrot(irotq);
                    tmpV *= broll;

                    torque.X += tmpV.X;
                    torque.Y += tmpV.Y;
                    torque.Z += tmpV.Z;

                    m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
                    m_amdampY = m_amdampZ;

                }
                else
                {
                    m_amdampZ = 1 / m_angularFrictionTimescale.Z;
                    m_amdampY = m_amdampX;
                }
            }
            else
            {
                m_ampwr = 1.0f;
                m_amdampX = 1 / m_angularFrictionTimescale.X;
                m_amdampY = 1 / m_angularFrictionTimescale.Y;
                m_amdampZ = 1 / m_angularFrictionTimescale.Z;
            }

            if(mousemode)
            {
                CameraData cam = rootPrim.TryGetCameraData();
                if(cam.Valid && cam.MouseLook)
                {
                    Vector3 dirv = cam.CameraAtAxis * irotq;

                    float invamts = 1.0f/m_angularMotorTimescale;
                    float tmp;

                    // get out of x == 0 plane
                    if(Math.Abs(dirv.X) < 0.001f)
                        dirv.X = 0.001f;

                    if (Math.Abs(dirv.Z) > 0.01)
                    {
                        tmp = -(float)Math.Atan2(dirv.Z, dirv.X) * m_angularMotorDirection.Y;
                        if(tmp < -4f)
                            tmp = -4f;
                        else if(tmp > 4f)
                            tmp = 4f;
                        torque.Y += (tmp - curLocalAngVel.Y) * invamts;
                        torque.Y -= curLocalAngVel.Y * m_amdampY;
                    }
                    else
                        torque.Y -= curLocalAngVel.Y * m_invtimestep;

                    if (Math.Abs(dirv.Y) > 0.01)
                    {
                        if(mousemodebank)
                        {
                            tmp = -(float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.X;
                            if(tmp < -4f)
                                tmp = -4f;
                            else if(tmp > 4f)
                                tmp = 4f;
                            torque.X += (tmp - curLocalAngVel.X) * invamts;
                        }
                        else
                        {
                            tmp = (float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.Z;
                            tmp *= invamts;
                            if(tmp < -4f)
                                tmp = -4f;
                            else if(tmp > 4f)
                                tmp = 4f;
                            torque.Z += (tmp - curLocalAngVel.Z) * invamts;
                        }
                        torque.X -= curLocalAngVel.X * m_amdampX;
                        torque.Z -= curLocalAngVel.Z * m_amdampZ;
                    }
                    else
                    {
                        if(mousemodebank)
                            torque.X -= curLocalAngVel.X * m_invtimestep;
                        else
                            torque.Z -= curLocalAngVel.Z * m_invtimestep;
                    }
                }
                else
                {
                    if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
                    {
                        torque.X -= curLocalAngVel.X * 10f;
                        torque.Y -= curLocalAngVel.Y * 10f;
                        torque.Z -= curLocalAngVel.Z * 10f;
                    }
                }
            }
            else
            {
                // angular motor
                if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
                {
                    tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
                    tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
                    torque.X += tmpV.X * m_ampwr;
                    torque.Y += tmpV.Y * m_ampwr;
                    torque.Z += tmpV.Z;

                    m_amEfect *= m_amDecay;
                }
                else
                    m_amEfect = 0;

                // angular deflection
                if (m_angularDeflectionEfficiency > 0)
                {
                    Vector3 dirv;

                    if (curLocalVel.X > 0.01f)
                        dirv = curLocalVel;
                    else if (curLocalVel.X < -0.01f)
                        // use oposite
                        dirv = -curLocalVel;
                    else
                    {
                        // make it fall into small positive x case
                        dirv.X = 0.01f;
                        dirv.Y = curLocalVel.Y;
                        dirv.Z = curLocalVel.Z;
                    }

                    float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;

                    if (Math.Abs(dirv.Z) > 0.01)
                    {
                        torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
                    }

                    if (Math.Abs(dirv.Y) > 0.01)
                    {
                        torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
                    }
                }

                if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
                {
                    torque.X -= curLocalAngVel.X * m_amdampX;
                    torque.Y -= curLocalAngVel.Y * m_amdampY;
                    torque.Z -= curLocalAngVel.Z * m_amdampZ;
                }
            }

            force *= dmass.mass;

            force += rootPrim.m_force;
            force += rootPrim.m_forceacc;
            rootPrim.m_forceacc = Vector3.Zero;

            if (force.X != 0 || force.Y != 0 || force.Z != 0)
            {
                SafeNativeMethods.BodyAddForce(Body, force.X, force.Y, force.Z);
            }

            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
            {
                torque *= m_referenceFrame; // to object frame
                dtorque.X = torque.X ;
                dtorque.Y = torque.Y;
                dtorque.Z = torque.Z;

                SafeNativeMethods.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
                SafeNativeMethods.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
            }

            torque = rootPrim.m_torque;
            torque += rootPrim.m_angularForceacc;
            rootPrim.m_angularForceacc = Vector3.Zero;
            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
                SafeNativeMethods.BodyAddTorque(Body,torque.X, torque.Y, torque.Z);
        }
    }
}