/* 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 March 5th 2010 by Kitto Flora. ODEDynamics.cs
 * Ubit 2012
 * rolled into ODEPrim.cs
 */

using System;
using System.IO;
using System.Collections.Generic;
using System.Reflection;
using System.Runtime.InteropServices;
using System.Threading;
using log4net;
using OpenMetaverse;
using Ode.NET;
using OpenSim.Framework;
using OpenSim.Region.Physics.Manager;

namespace OpenSim.Region.Physics.OdePlugin
{
    /// <summary>
    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
    /// </summary>

    public class OdePrim : PhysicsActor
    {
        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

        public class SerialControl
        {
            public object alock = new object();
            public byte[] data = new byte[0];
        }
        private Vector3 _position;
        private Vector3 _velocity;
        private Vector3 _torque;
        private Vector3 m_lastVelocity;
        private Vector3 m_lastposition;
        private Quaternion m_lastorientation = new Quaternion();
        private Vector3 m_rotationalVelocity;
        private Vector3 _size;
        private Vector3 _acceleration;
        // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
        private Quaternion _orientation;
        private Vector3 m_taintposition;
        private Vector3 m_taintsize;
        private Vector3 m_taintVelocity;
        private Vector3 m_taintTorque;
        private Quaternion m_taintrot;
        private Vector3 m_rotateEnable = Vector3.One;				// Current setting
        private Vector3 m_rotateEnableRequest = Vector3.One;			// Request from LSL
        private bool m_rotateEnableUpdate = false;
        private Vector3 m_lockX;
        private Vector3 m_lockY;
        private Vector3 m_lockZ;
        private IntPtr Amotor = IntPtr.Zero;
        private IntPtr AmotorX = IntPtr.Zero;
        private IntPtr AmotorY = IntPtr.Zero;
        private IntPtr AmotorZ = IntPtr.Zero;

        private Vector3 m_PIDTarget;
        private float m_PIDTau;
        private float PID_D = 35f;
        private float PID_G = 25f;
        private bool m_usePID = false;

        private Quaternion m_APIDTarget = new Quaternion();
        private float m_APIDStrength = 0.5f;
        private float m_APIDDamping = 0.5f;
        private bool m_useAPID = false;
        private float m_APIDdamper = 1.0f;

        // 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;
        private bool m_useHoverPID;
        private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
        private float m_targetHoverHeight;
        private float m_groundHeight;
        private float m_waterHeight;
        private float m_buoyancy;				//m_buoyancy set by llSetBuoyancy() 

        // private float m_tensor = 5f;
        private int body_autodisable_frames = 20;


        private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
                                                        | CollisionCategories.Space
                                                        | CollisionCategories.Body
                                                        | CollisionCategories.Character
                                                        );
        private bool m_taintshape;
        private bool m_taintPhysics;
        private bool m_collidesLand = true;
        private bool m_collidesWater;
        //        public bool m_returnCollisions;

        // Default we're a Geometry
        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);

        // Default, Collide with Other Geometries, spaces and Bodies
        private CollisionCategories m_collisionFlags = m_default_collisionFlags;

        public bool m_taintremove;
        public bool m_taintdisable;
        public bool m_disabled;
        public bool m_taintadd;
        public bool m_taintselected;
        public bool m_taintCollidesWater;

        public uint m_localID;

        //public GCHandle gc;
        private CollisionLocker ode;

        private bool m_meshfailed = false;
        private bool m_taintforce = false;
        private bool m_taintaddangularforce = false;
        private Vector3 m_force;
        private List<Vector3> m_forcelist = new List<Vector3>();
        private List<Vector3> m_angularforcelist = new List<Vector3>();

        private IMesh _mesh;
        private PrimitiveBaseShape _pbs;
        private OdeScene _parent_scene;
        public IntPtr m_targetSpace = IntPtr.Zero;
        public IntPtr prim_geom;
        //        public IntPtr prev_geom;
        public IntPtr _triMeshData;

        private IntPtr _linkJointGroup = IntPtr.Zero;
        private PhysicsActor _parent;
        private PhysicsActor m_taintparent;

        private List<OdePrim> childrenPrim = new List<OdePrim>();

        private bool iscolliding;
        private bool m_isphysical;
        private bool m_isSelected;

        private bool m_NoColide;  // for now only for internal use for bad meshs

        internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively

        private bool m_throttleUpdates;
        private int throttleCounter;
        public int m_interpenetrationcount;
        public float m_collisionscore;
        //        public int m_roundsUnderMotionThreshold;
        //        private int m_crossingfailures;

        public bool m_outofBounds;
        private float m_density = 10.000006836f; // Aluminum g/cm3;

        public bool _zeroFlag;					 // if body has been stopped
        private bool m_lastUpdateSent;

        public IntPtr Body = IntPtr.Zero;
        public String m_primName;
        private Vector3 _target_velocity;
        public d.Mass pMass;

        public int m_eventsubscription;
        private CollisionEventUpdate CollisionEventsThisFrame;

        private IntPtr m_linkJoint = IntPtr.Zero;

        public volatile bool childPrim;

        internal int m_material = (int)Material.Wood;

        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

        //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.

//        SerialControl m_taintserial = null;
        object m_taintvehicledata = null;

        public void DoSetVehicle()
        {
            VehicleData vd = (VehicleData)m_taintvehicledata;

        m_type = vd.m_type;
        m_flags = vd.m_flags;

        // Linear properties
        m_linearMotorDirection = vd.m_linearMotorDirection;
        m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
        m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
        m_linearMotorTimescale = vd.m_linearMotorTimescale;
//        m_linearMotorOffset = vd.m_linearMotorOffset;

        //Angular properties
        m_angularMotorDirection = vd.m_angularMotorDirection;
        m_angularMotorTimescale = vd.m_angularMotorTimescale;
        m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
        m_angularFrictionTimescale = vd.m_angularFrictionTimescale;

        //Deflection properties
//        m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
//        m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
//        m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
//        m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;

        //Banking properties
//        m_bankingEfficiency = vd.m_bankingEfficiency;
//        m_bankingMix = vd.m_bankingMix;
//        m_bankingTimescale = vd.m_bankingTimescale;

        //Hover and Buoyancy properties
        m_VhoverHeight = vd.m_VhoverHeight;
//        m_VhoverEfficiency = vd.m_VhoverEfficiency;
        m_VhoverTimescale = vd.m_VhoverTimescale;
        m_VehicleBuoyancy = vd.m_VehicleBuoyancy;

        //Attractor properties
        m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
        m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;

        // Axis
//        m_referenceFrame = vd.m_referenceFrame;


            m_taintvehicledata = null;
        }

        public override void SetVehicle(object vdata)
        {
            m_taintvehicledata = vdata;
            _parent_scene.AddPhysicsActorTaint(this);
        }
        public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
                       Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid)
        {
            m_localID = localid;
            ode = dode;
            if (!pos.IsFinite())
            {
                pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
                    parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
                m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
            }

            _position = pos;
            m_taintposition = pos;
            PID_D = parent_scene.bodyPIDD;
            PID_G = parent_scene.bodyPIDG;
            m_density = parent_scene.geomDefaultDensity;
            // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;

            prim_geom = IntPtr.Zero;
            //            prev_geom = IntPtr.Zero;

            if (!pos.IsFinite())
            {
                size = new Vector3(0.5f, 0.5f, 0.5f);
                m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
            }

            if (size.X <= 0) size.X = 0.01f;
            if (size.Y <= 0) size.Y = 0.01f;
            if (size.Z <= 0) size.Z = 0.01f;

            _size = size;
            m_taintsize = _size;

            if (!QuaternionIsFinite(rotation))
            {
                rotation = Quaternion.Identity;
                m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
            }

            _orientation = rotation;
            m_taintrot = _orientation;
            _mesh = mesh;
            _pbs = pbs;

            _parent_scene = parent_scene;
            m_targetSpace = (IntPtr)0;

            //            if (pos.Z < 0)
            if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
                m_isphysical = false;
            else
            {
                m_isphysical = pisPhysical;
                // If we're physical, we need to be in the master space for now.
                // linksets *should* be in a space together..  but are not currently
                if (m_isphysical)
                    m_targetSpace = _parent_scene.space;
            }

            _triMeshData = IntPtr.Zero;
            m_NoColide = false;

//            m_taintserial = null;
            m_primName = primName;
            m_taintadd = true;
            _parent_scene.AddPhysicsActorTaint(this);
            //  don't do .add() here; old geoms get recycled with the same hash
        }

        public override int PhysicsActorType
        {
            get { return (int)ActorTypes.Prim; }
            set { return; }
        }

        public override bool SetAlwaysRun
        {
            get { return false; }
            set { return; }
        }

        public override uint LocalID
        {
            set
            {
                //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
                m_localID = value;
            }
        }

        public override bool Grabbed
        {
            set { return; }
        }

        public override bool Selected
        {
            set
            {
                //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
                // through it while it's selected
                m_collisionscore = 0;
                if ((m_isphysical && !_zeroFlag) || !value)
                {
                    m_taintselected = value;
                    _parent_scene.AddPhysicsActorTaint(this);
                }
                else
                {
                    m_taintselected = value;
                    m_isSelected = value;
                }
                if (m_isSelected) disableBodySoft();
            }
        }

        public override bool IsPhysical
        {
            get { return m_isphysical; }
            set
            {
                m_isphysical = value;
                if (!m_isphysical)
                {		// Zero the remembered last velocity
                    m_lastVelocity = Vector3.Zero;
                    if (m_type != Vehicle.TYPE_NONE) Halt();
                }
            }
        }

        public void setPrimForRemoval()
        {
            m_taintremove = true;
        }

        public override bool Flying
        {
            // no flying prims for you
            get { return false; }
            set { }
        }

        public override bool IsColliding
        {
            get { return iscolliding; }
            set { iscolliding = value; }
        }

        public override bool CollidingGround
        {
            get { return false; }
            set { return; }
        }

        public override bool CollidingObj
        {
            get { return false; }
            set { return; }
        }

        public override bool ThrottleUpdates
        {
            get { return m_throttleUpdates; }
            set { m_throttleUpdates = value; }
        }

        public override bool Stopped
        {
            get { return _zeroFlag; }
        }

        public override Vector3 Position
        {
            get { return _position; }

            set
            {
                _position = value;
                //m_log.Info("[PHYSICS]: " + _position.ToString());
            }
        }

        public override Vector3 Size
        {
            get { return _size; }
            set
            {
                if (value.IsFinite())
                {
                    _size = value;
                }
                else
                {
                    m_log.Warn("[PHYSICS]: Got NaN Size on object");
                }
            }
        }

        public override float Mass
        {
            get { return CalculateMass(); }
        }

        public override Vector3 Force
        {
            //get { return Vector3.Zero; }
            get { return m_force; }
            set
            {
                if (value.IsFinite())
                {
                    m_force = value;
                }
                else
                {
                    m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
                }
            }
        }

        public override int VehicleType
        {
            get { return (int)m_type; }
            set { ProcessTypeChange((Vehicle)value); }
        }

        public override void VehicleFloatParam(int param, float value)
        {
            ProcessFloatVehicleParam((Vehicle)param, value);
        }

        public override void VehicleVectorParam(int param, Vector3 value)
        {
            ProcessVectorVehicleParam((Vehicle)param, value);
        }

        public override void VehicleRotationParam(int param, Quaternion rotation)
        {
            ProcessRotationVehicleParam((Vehicle)param, rotation);
        }

        public override void VehicleFlags(int param, bool remove)
        {
            ProcessVehicleFlags(param, remove);
        }

        public override void SetVolumeDetect(int param)
        {
            lock (_parent_scene.OdeLock)
            {
                m_isVolumeDetect = (param != 0);
            }
        }

        public override Vector3 CenterOfMass
        {
            get { return Vector3.Zero; }
        }

        public override Vector3 GeometricCenter
        {
            get { return Vector3.Zero; }
        }

        public override PrimitiveBaseShape Shape
        {
            set
            {
                _pbs = value;
                m_taintshape = true;
            }
        }

        public override Vector3 Velocity
        {
            get
            {
                // Averate previous velocity with the new one so
                // client object interpolation works a 'little' better
                if (_zeroFlag)
                    return Vector3.Zero;

                Vector3 returnVelocity = Vector3.Zero;
                returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
                return returnVelocity;
            }
            set
            {
                if (value.IsFinite())
                {
                    _velocity = value;
                    if (_velocity.ApproxEquals(Vector3.Zero, 0.001f))
                        _acceleration = Vector3.Zero;

                    m_taintVelocity = value;
                    _parent_scene.AddPhysicsActorTaint(this);
                }
                else
                {
                    m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
                }

            }
        }

        public override Vector3 Torque
        {
            get
            {
                if (!m_isphysical || Body == IntPtr.Zero)
                    return Vector3.Zero;

                return _torque;
            }

            set
            {
                if (value.IsFinite())
                {
                    m_taintTorque = value;
                    _parent_scene.AddPhysicsActorTaint(this);
                }
                else
                {
                    m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
                }
            }
        }

        public override float CollisionScore
        {
            get { return m_collisionscore; }
            set { m_collisionscore = value; }
        }

        public override bool Kinematic
        {
            get { return false; }
            set { }
        }

        public override Quaternion Orientation
        {
            get { return _orientation; }
            set
            {
                if (QuaternionIsFinite(value))
                {
                    _orientation = value;
                }
                else
                    m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");

            }
        }

        public override bool FloatOnWater
        {
            set
            {
                m_taintCollidesWater = value;
                _parent_scene.AddPhysicsActorTaint(this);
            }
        }

        public override void SetMomentum(Vector3 momentum)
        {
        }

        public override Vector3 PIDTarget
        {
            set
            {
                if (value.IsFinite())
                {
                    m_PIDTarget = value;
                }
                else
                    m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
            }
        }
        public override bool PIDActive { set { m_usePID = value; } }
        public override float PIDTau { set { m_PIDTau = value; } }

        // For RotLookAt        
        public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
        public override bool APIDActive { set { m_useAPID = value; } }
        public override float APIDStrength { set { m_APIDStrength = value; } }
        public override float APIDDamping { set { m_APIDDamping = value; } }

        public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
        public override bool PIDHoverActive { set { m_useHoverPID = value; } }
        public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
        public override float PIDHoverTau { set { m_PIDHoverTau = value; } }

        internal static bool QuaternionIsFinite(Quaternion q)
        {
            if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
                return false;
            if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
                return false;
            if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
                return false;
            if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
                return false;
            return true;
        }

        public override Vector3 Acceleration		// client updates read data via here
        {
            get
            {
                if (_zeroFlag)
                {
                    return Vector3.Zero;
                }
                return _acceleration;
            }
            set { _acceleration = value; }
        }


        public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
        {
            _acceleration = accel;
        }

        public override void AddForce(Vector3 force, bool pushforce)
        {
            if (force.IsFinite())
            {
                lock (m_forcelist)
                    m_forcelist.Add(force);

                m_taintforce = true;
            }
            else
            {
                m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
            }
            //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
        }

        public override void AddAngularForce(Vector3 force, bool pushforce)
        {
            if (force.IsFinite())
            {
                m_angularforcelist.Add(force);
                m_taintaddangularforce = true;
            }
            else
            {
                m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
            }
        }

        public override Vector3 RotationalVelocity
        {
            get
            {
                return m_rotationalVelocity;
            }
            set
            {
                if (value.IsFinite())
                {
                    m_rotationalVelocity = value;
                }
                else
                {
                    m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
                }
            }
        }

        public override void CrossingFailure()
        {
            if (m_outofBounds)
            {
                _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
                _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
                _position.Z = Util.Clip(_position.Z, -100f, 50000f);
                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);

                m_lastposition = _position;

                _velocity = Vector3.Zero;
                m_lastVelocity = _velocity;


                if (m_type != Vehicle.TYPE_NONE)
                    Halt();

                d.BodySetLinearVel(Body, 0, 0, 0);
                base.RequestPhysicsterseUpdate();
                m_outofBounds = false;
            }
            /*
                        int tmp = Interlocked.Increment(ref m_crossingfailures);
                        if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
                        {
                            base.RaiseOutOfBounds(_position);
                            return;
                        }
                        else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
                        {
                            m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
                        }
             */
        }

        public override float Buoyancy
        {
            get { return m_buoyancy; }
            set { m_buoyancy = value; }
        }

        public override void link(PhysicsActor obj)
        {
            m_taintparent = obj;
        }

        public override void delink()
        {
            m_taintparent = null;
        }

        public override void LockAngularMotion(Vector3 axis)
        {
            // This is actually ROTATION ENABLE, not a lock.
            // default is <1,1,1> which is all enabled. 
            // The lock value is updated inside Move(), no point in using the taint system.
            // OS 'm_taintAngularLock' etc change to m_rotateEnable.
            if (axis.IsFinite())
            {
                axis.X = (axis.X > 0) ? 1f : 0f;
                axis.Y = (axis.Y > 0) ? 1f : 0f;
                axis.Z = (axis.Z > 0) ? 1f : 0f;
                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
                m_rotateEnableRequest = axis;
                m_rotateEnableUpdate = true;
            }
            else
            {
                m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
            }
        }

        public void SetGeom(IntPtr geom)
        {
            if (prim_geom != IntPtr.Zero)
            {
                // Remove any old entries
                //string tPA;
                //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
                //Console.WriteLine("**** Remove {0}", tPA);
                if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
                if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
                d.GeomDestroy(prim_geom);
            }

            prim_geom = geom;
            //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);     
            if (prim_geom != IntPtr.Zero)
            {
                _parent_scene.geom_name_map[prim_geom] = this.m_primName;
                _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
                //Console.WriteLine("**** Create {2}    Dicts: actor={0}   name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
                if (m_NoColide)
                {
                    d.GeomSetCategoryBits(prim_geom, 0);
                    if (m_isphysical)
                    {
                        d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
                    }
                    else
                    {
                        d.GeomSetCollideBits(prim_geom, 0);
                        d.GeomDisable(prim_geom);
                    }
                }
                else
                {
                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                }
            }

            if (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);
                Halt();
            }
        }

        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);

                if (m_NoColide)
                {
                    d.GeomSetCategoryBits(prim_geom, 0);
                    d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
                }
                else
                {
                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                }

                d.BodySetAutoDisableFlag(Body, true);
                d.BodySetAutoDisableSteps(Body, body_autodisable_frames);

                // 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 = _size.X * _size.Y * _size.Z; // default
            float tmp;

            float returnMass = 0;
            float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
            float hollowVolume = hollowAmount * hollowAmount;

            switch (_pbs.ProfileShape)
            {
                case ProfileShape.Square:
                    // default box

                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
                    {
                        if (hollowAmount > 0.0)
                        {
                            switch (_pbs.HollowShape)
                            {
                                case HollowShape.Square:
                                case HollowShape.Same:
                                    break;

                                case HollowShape.Circle:

                                    hollowVolume *= 0.78539816339f;
                                    break;

                                case HollowShape.Triangle:

                                    hollowVolume *= (0.5f * .5f);
                                    break;

                                default:
                                    hollowVolume = 0;
                                    break;
                            }
                            volume *= (1.0f - hollowVolume);
                        }
                    }

                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
                    {
                        //a tube 

                        volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
                        tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
                        volume -= volume * tmp * tmp;

                        if (hollowAmount > 0.0)
                        {
                            hollowVolume *= hollowAmount;

                            switch (_pbs.HollowShape)
                            {
                                case HollowShape.Square:
                                case HollowShape.Same:
                                    break;

                                case HollowShape.Circle:
                                    hollowVolume *= 0.78539816339f; ;
                                    break;

                                case HollowShape.Triangle:
                                    hollowVolume *= 0.5f * 0.5f;
                                    break;
                                default:
                                    hollowVolume = 0;
                                    break;
                            }
                            volume *= (1.0f - hollowVolume);
                        }
                    }

                    break;

                case ProfileShape.Circle:

                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
                    {
                        volume *= 0.78539816339f; // elipse base

                        if (hollowAmount > 0.0)
                        {
                            switch (_pbs.HollowShape)
                            {
                                case HollowShape.Same:
                                case HollowShape.Circle:
                                    break;

                                case HollowShape.Square:
                                    hollowVolume *= 0.5f * 2.5984480504799f;
                                    break;

                                case HollowShape.Triangle:
                                    hollowVolume *= .5f * 1.27323954473516f;
                                    break;

                                default:
                                    hollowVolume = 0;
                                    break;
                            }
                            volume *= (1.0f - hollowVolume);
                        }
                    }

                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
                    {
                        volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
                        volume *= (1.0f - tmp * tmp);

                        if (hollowAmount > 0.0)
                        {

                            // calculate the hollow volume by it's shape compared to the prim shape
                            hollowVolume *= hollowAmount;

                            switch (_pbs.HollowShape)
                            {
                                case HollowShape.Same:
                                case HollowShape.Circle:
                                    break;

                                case HollowShape.Square:
                                    hollowVolume *= 0.5f * 2.5984480504799f;
                                    break;

                                case HollowShape.Triangle:
                                    hollowVolume *= .5f * 1.27323954473516f;
                                    break;

                                default:
                                    hollowVolume = 0;
                                    break;
                            }
                            volume *= (1.0f - hollowVolume);
                        }
                    }
                    break;

                case ProfileShape.HalfCircle:
                    if (_pbs.PathCurve == (byte)Extrusion.Curve1)
                    {
                        volume *= 0.52359877559829887307710723054658f;
                    }
                    break;

                case ProfileShape.EquilateralTriangle:

                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
                    {
                        volume *= 0.32475953f;

                        if (hollowAmount > 0.0)
                        {

                            // calculate the hollow volume by it's shape compared to the prim shape
                            switch (_pbs.HollowShape)
                            {
                                case HollowShape.Same:
                                case HollowShape.Triangle:
                                    hollowVolume *= .25f;
                                    break;

                                case HollowShape.Square:
                                    hollowVolume *= 0.499849f * 3.07920140172638f;
                                    break;

                                case HollowShape.Circle:
                                    // Hollow shape is a perfect cyllinder in respect to the cube's scale
                                    // Cyllinder hollow volume calculation

                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
                                    break;

                                default:
                                    hollowVolume = 0;
                                    break;
                            }
                            volume *= (1.0f - hollowVolume);
                        }
                    }
                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
                    {
                        volume *= 0.32475953f;
                        volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
                        volume *= (1.0f - tmp * tmp);

                        if (hollowAmount > 0.0)
                        {

                            hollowVolume *= hollowAmount;

                            switch (_pbs.HollowShape)
                            {
                                case HollowShape.Same:
                                case HollowShape.Triangle:
                                    hollowVolume *= .25f;
                                    break;

                                case HollowShape.Square:
                                    hollowVolume *= 0.499849f * 3.07920140172638f;
                                    break;

                                case HollowShape.Circle:

                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
                                    break;

                                default:
                                    hollowVolume = 0;
                                    break;
                            }
                            volume *= (1.0f - hollowVolume);
                        }
                    }
                    break;

                default:
                    break;
            }



            float taperX1;
            float taperY1;
            float taperX;
            float taperY;
            float pathBegin;
            float pathEnd;
            float profileBegin;
            float profileEnd;

            if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
            {
                taperX1 = _pbs.PathScaleX * 0.01f;
                if (taperX1 > 1.0f)
                    taperX1 = 2.0f - taperX1;
                taperX = 1.0f - taperX1;

                taperY1 = _pbs.PathScaleY * 0.01f;
                if (taperY1 > 1.0f)
                    taperY1 = 2.0f - taperY1;
                taperY = 1.0f - taperY1;
            }
            else
            {
                taperX = _pbs.PathTaperX * 0.01f;
                if (taperX < 0.0f)
                    taperX = -taperX;
                taperX1 = 1.0f - taperX;

                taperY = _pbs.PathTaperY * 0.01f;
                if (taperY < 0.0f)
                    taperY = -taperY;
                taperY1 = 1.0f - taperY;

            }


            volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);

            pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
            pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
            volume *= (pathEnd - pathBegin);

            // this is crude aproximation
            profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
            profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
            volume *= (profileEnd - profileBegin);

            returnMass = m_density * volume;

            if (returnMass <= 0)
                returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
            //            else if (returnMass > _parent_scene.maximumMassObject)
            //                returnMass = _parent_scene.maximumMassObject;




            // Recursively calculate mass
            bool HasChildPrim = false;
            lock (childrenPrim)
            {
                if (childrenPrim.Count > 0)
                {
                    HasChildPrim = true;
                }

            }
            if (HasChildPrim)
            {
                OdePrim[] childPrimArr = new OdePrim[0];

                lock (childrenPrim)
                    childPrimArr = childrenPrim.ToArray();

                for (int i = 0; i < childPrimArr.Length; i++)
                {
                    if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
                        returnMass += childPrimArr[i].CalculateMass();
                    // failsafe, this shouldn't happen but with OpenSim, you never know :)
                    if (i > 256)
                        break;
                }
            }
            if (returnMass > _parent_scene.maximumMassObject)
                returnMass = _parent_scene.maximumMassObject;
            return returnMass;
        }// end CalculateMass

        #endregion

        public void setMass()
        {
            if (Body != (IntPtr)0)
            {
                float newmass = CalculateMass();

                //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());

                d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
                d.BodySetMass(Body, ref pMass);
            }
        }

        public void disableBody()
        {
            //this kills the body so things like 'mesh' can re-create it.
            lock (this)
            {
                if (!childPrim)
                {
                    if (Body != IntPtr.Zero)
                    {
                        _parent_scene.remActivePrim(this);
                        m_collisionCategories &= ~CollisionCategories.Body;
                        m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);

                        if (prim_geom != IntPtr.Zero)
                        {
                            if (m_NoColide)
                            {
                                d.GeomSetCategoryBits(prim_geom, 0);
                                d.GeomSetCollideBits(prim_geom, 0);
                                d.GeomDisable(prim_geom);
                            }
                            else
                            {
                                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                            }
                        }

                        d.BodyDestroy(Body);
                        lock (childrenPrim)
                        {
                            if (childrenPrim.Count > 0)
                            {
                                foreach (OdePrim prm in childrenPrim)
                                {
                                    if (prm.m_NoColide && prm.prim_geom != IntPtr.Zero)
                                    {
                                        d.GeomSetCategoryBits(prm.prim_geom, 0);
                                        d.GeomSetCollideBits(prm.prim_geom, 0);
                                        d.GeomDisable(prm.prim_geom);
                                    }

                                    _parent_scene.remActivePrim(prm);
                                    prm.Body = IntPtr.Zero;
                                }
                            }
                        }
                        Body = IntPtr.Zero;
                    }
                }
                else
                {
                    _parent_scene.remActivePrim(this);

                    m_collisionCategories &= ~CollisionCategories.Body;
                    m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);

                    if (prim_geom != IntPtr.Zero)
                    {
                        if (m_NoColide)
                        {
                            d.GeomSetCategoryBits(prim_geom, 0);
                            d.GeomSetCollideBits(prim_geom, 0);
                            d.GeomDisable(prim_geom);
                        }
                        else
                        {
                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                        }

                    }


                    Body = IntPtr.Zero;
                }
            }
            m_disabled = true;
            m_collisionscore = 0;
        }

        private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();

        public bool setMesh(OdeScene parent_scene, IMesh mesh)
        {
            //Kill Body so that mesh can re-make the geom
            if (IsPhysical && Body != IntPtr.Zero)
            {
                if (childPrim)
                {
                    if (_parent != null)
                    {
                        OdePrim parent = (OdePrim)_parent;
                        parent.ChildDelink(this);
                    }
                }
                else
                {
                    disableBody();
                }
            }

            IntPtr vertices, indices;
            int vertexCount, indexCount;
            int vertexStride, triStride;
            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage

            // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid
            mesh.releaseSourceMeshData(); // free up the original mesh data to save memory

            if (vertexCount == 0 || indexCount == 0)
            {
                m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
                return false;
            }

            IntPtr geo = IntPtr.Zero;
            try
            {
                _triMeshData = d.GeomTriMeshDataCreate();
                d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
                d.GeomTriMeshDataPreprocess(_triMeshData);

                _parent_scene.waitForSpaceUnlock(m_targetSpace);

                 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
            }
            catch (Exception e)
            {
                m_log.ErrorFormat("[PHYSICS]: Create trimesh failed on prim {0} : {1}",Name,e.Message);

                if (_triMeshData != IntPtr.Zero)
                {
                    d.GeomTriMeshDataDestroy(_triMeshData);
                    _triMeshData = IntPtr.Zero;
                }
                return false;
            }

            SetGeom(geo);

            return true;
        }

        public void ProcessTaints(float timestep) //=============================================================================
        {
            if (m_taintadd)
            {
                changeadd(timestep);
            }

            if (m_taintremove)
                return;

            if (prim_geom != IntPtr.Zero)
            {
                if (!_position.ApproxEquals(m_taintposition, 0f))
                {
                    changemove(timestep);
                }
                if (m_taintrot != _orientation)
                {
                    if (childPrim && IsPhysical)	// For physical child prim...
                    {
                        rotate(timestep);
                        // KF: ODE will also rotate the parent prim!
                        // so rotate the root back to where it was
                        OdePrim parent = (OdePrim)_parent;
                        parent.rotate(timestep);
                    }
                    else
                    {
                        //Just rotate the prim
                        rotate(timestep);
                    }
                }
                //

                if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
                {
                    changePhysicsStatus(timestep);
                }//

                if (!_size.ApproxEquals(m_taintsize, 0f))
                    changesize(timestep);
                //

                if (m_taintshape)
                    changeshape(timestep);
                //

                if (m_taintforce)
                    changeAddForce(timestep);

                if (m_taintaddangularforce)
                    changeAddAngularForce(timestep);

                if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
                    changeSetTorque(timestep);

                if (m_taintdisable)
                    changedisable(timestep);

                if (m_taintselected != m_isSelected)
                    changeSelectedStatus(timestep);

                if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
                    changevelocity(timestep);

                if (m_taintparent != _parent)
                    changelink(timestep);

                if (m_taintCollidesWater != m_collidesWater)
                    changefloatonwater(timestep);

                if (m_taintvehicledata != null)
                    DoSetVehicle();

                /* obsolete                    
                                if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
                                    changeAngularLock(timestep);
                 */
            }

            else
            {
                m_log.Error("[PHYSICS]: prim {0} at <{1},{2},{3}> as invalid geom");

                // not sure this will not flame...
                m_taintremove = true;
                _parent_scene.AddPhysicsActorTaint(this);
            }

        }

        /* obsolete
                private void changeAngularLock(float timestep) 
                {
                    if (_parent == null)
                    {
                        m_angularLock = m_taintAngularLock;
                        m_angularLockSet = true;
                    }
                }
         */
        private void changelink(float timestep)
        {
            // If the newly set parent is not null
            // create link
            if (_parent == null && m_taintparent != null)
            {
                if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
                {
                    OdePrim obj = (OdePrim)m_taintparent;
                    //obj.disableBody();
                    obj.ParentPrim(this);

                    /*
                    if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
                    {
                        _linkJointGroup = d.JointGroupCreate(0);
                        m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
                        d.JointAttach(m_linkJoint, obj.Body, Body);
                        d.JointSetFixed(m_linkJoint);
                    }
                     */
                }
            }
            // If the newly set parent is null
            // destroy link
            else if (_parent != null && m_taintparent == null)
            {
                if (_parent is OdePrim)
                {
                    OdePrim obj = (OdePrim)_parent;
                    obj.ChildDelink(this);
                    childPrim = false;
                    //_parent = null;
                }

                /*
                    if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
                    d.JointGroupDestroy(_linkJointGroup);
                        
                    _linkJointGroup = (IntPtr)0;
                    m_linkJoint = (IntPtr)0;
                */
            }

            _parent = m_taintparent;
            m_taintPhysics = m_isphysical;
        }

        // I'm the parent
        // prim is the child
        public void ParentPrim(OdePrim prim)
        {
            if (this.m_localID != prim.m_localID)
            {
                if (Body == IntPtr.Zero)
                {
                    Body = d.BodyCreate(_parent_scene.world);
                    // disconnect from world gravity so we can apply buoyancy
                    d.BodySetGravityMode(Body, false);

                    setMass();
                }
                if (Body != IntPtr.Zero)
                {
                    lock (childrenPrim)
                    {
                        if (!childrenPrim.Contains(prim))
                        {
                            childrenPrim.Add(prim);

                            foreach (OdePrim prm in childrenPrim)
                            {
                                d.Mass m2;
                                d.MassSetZero(out m2);
                                d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);


                                d.Quaternion quat = new d.Quaternion();
                                quat.W = prm._orientation.W;
                                quat.X = prm._orientation.X;
                                quat.Y = prm._orientation.Y;
                                quat.Z = prm._orientation.Z;

                                d.Matrix3 mat = new d.Matrix3();
                                d.RfromQ(out mat, ref quat);
                                d.MassRotate(ref m2, ref mat);
                                d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
                                d.MassAdd(ref pMass, ref m2);
                            }
                            foreach (OdePrim prm in childrenPrim)
                            {
                                prm.m_collisionCategories |= CollisionCategories.Body;
                                prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

                                if (prm.prim_geom == IntPtr.Zero)
                                {
                                    m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements.  No geom yet");
                                    continue;
                                }

                                if (prm.m_NoColide)
                                {
                                    d.GeomSetCategoryBits(prm.prim_geom, 0);
                                    d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
                                }
                                else
                                {
                                    d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
                                    d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
                                }

                                d.Quaternion quat = new d.Quaternion();
                                quat.W = prm._orientation.W;
                                quat.X = prm._orientation.X;
                                quat.Y = prm._orientation.Y;
                                quat.Z = prm._orientation.Z;

                                d.Matrix3 mat = new d.Matrix3();
                                d.RfromQ(out mat, ref quat);
                                if (Body != IntPtr.Zero)
                                {
                                    d.GeomSetBody(prm.prim_geom, Body);
                                    prm.childPrim = true;
                                    d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z);
                                    //d.GeomSetOffsetPosition(prim.prim_geom,
                                    //    (Position.X - prm.Position.X) - pMass.c.X,
                                    //    (Position.Y - prm.Position.Y) - pMass.c.Y,
                                    //    (Position.Z - prm.Position.Z) - pMass.c.Z);
                                    d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
                                    //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
                                    d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
                                    d.BodySetMass(Body, ref pMass);
                                }
                                else
                                {
                                    m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
                                }

                                prm.m_interpenetrationcount = 0;
                                prm.m_collisionscore = 0;
                                prm.m_disabled = false;

                                prm.Body = Body;

                                    _parent_scene.addActivePrim(prm);
                            }

                            m_collisionCategories |= CollisionCategories.Body;
                            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

                            if (m_NoColide)
                            {
                                d.GeomSetCategoryBits(prim_geom, 0);
                                d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
                            }
                            else
                            {
                                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                            }

                            d.Quaternion quat2 = new d.Quaternion();
                            quat2.W = _orientation.W;
                            quat2.X = _orientation.X;
                            quat2.Y = _orientation.Y;
                            quat2.Z = _orientation.Z;

                            d.Matrix3 mat2 = new d.Matrix3();
                            d.RfromQ(out mat2, ref quat2);
                            d.GeomSetBody(prim_geom, Body);
                            d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
                            //d.GeomSetOffsetPosition(prim.prim_geom,
                            //    (Position.X - prm.Position.X) - pMass.c.X,
                            //    (Position.Y - prm.Position.Y) - pMass.c.Y,
                            //    (Position.Z - prm.Position.Z) - pMass.c.Z);
                            //d.GeomSetOffsetRotation(prim_geom, ref mat2);
                            d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
                            d.BodySetMass(Body, ref pMass);

                            d.BodySetAutoDisableFlag(Body, true);
                            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);

                            m_interpenetrationcount = 0;
                            m_collisionscore = 0;
                            m_disabled = false;

                            d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
                            if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);

                            _parent_scene.addActivePrim(this);
                        }
                    }
                }
            }
        }

        private void ChildSetGeom(OdePrim odePrim)
        {
            //if (m_isphysical && Body != IntPtr.Zero)
            lock (childrenPrim)
            {
                foreach (OdePrim prm in childrenPrim)
                {
                    //prm.childPrim = true;
                    prm.disableBody();
                    //prm.m_taintparent = null;
                    //prm._parent = null;
                    //prm.m_taintPhysics = false;
                    //prm.m_disabled = true;
                    //prm.childPrim = false;
                }
            }
            disableBody();


            if (Body != IntPtr.Zero)
            {
                _parent_scene.remActivePrim(this);
            }

            lock (childrenPrim)
            {
                foreach (OdePrim prm in childrenPrim)
                {
                    ParentPrim(prm);
                }
            }

        }

        private void ChildDelink(OdePrim odePrim)
        {
            // Okay, we have a delinked child..   need to rebuild the body.
            lock (childrenPrim)
            {
                foreach (OdePrim prm in childrenPrim)
                {
                    prm.childPrim = true;
                    prm.disableBody();
                    //prm.m_taintparent = null;
                    //prm._parent = null;
                    //prm.m_taintPhysics = false;
                    //prm.m_disabled = true;
                    //prm.childPrim = false;
                }
            }
            disableBody();

            lock (childrenPrim)
            {
                childrenPrim.Remove(odePrim);
            }

            if (Body != IntPtr.Zero)
            {
                _parent_scene.remActivePrim(this);
            }

            lock (childrenPrim)
            {
                foreach (OdePrim prm in childrenPrim)
                {
                    ParentPrim(prm);
                }
            }
        }

        private void changeSelectedStatus(float timestep)
        {
            if (m_taintselected)
            {
                m_collisionCategories = CollisionCategories.Selected;
                m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);

                // We do the body disable soft twice because 'in theory' a collision could have happened
                // in between the disabling and the collision properties setting
                // which would wake the physical body up from a soft disabling and potentially cause it to fall
                // through the ground.

                // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
                // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
                // so that causes the selected part to wake up and continue moving.

                // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
                // assembly will stop simulating during the selection, because of the lack of atomicity
                // of select operations (their processing could be interrupted by a thread switch, causing
                // simulation to continue before all of the selected object notifications trickle down to
                // the physics engine).

                // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
                // selected and disabled. then, due to a thread switch, the selection processing is
                // interrupted and the physics engine continues to simulate, so the last 50 items, whose
                // selection was not yet processed, continues to simulate. this wakes up ALL of the 
                // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
                // up, start simulating again, which in turn wakes up the last 50.

                if (m_isphysical)
                {
                    disableBodySoft();
                }

                if (prim_geom != IntPtr.Zero)
                {
                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                    if (m_NoColide)
                        d.GeomDisable(prim_geom);
                }

                if (m_isphysical)
                {
                    disableBodySoft();
                }
                if (Body != IntPtr.Zero)
                {
                    d.BodySetLinearVel(Body, 0f, 0f, 0f);
                    d.BodySetForce(Body, 0f, 0f, 0f);
                    d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
                    d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
                }

            }
            else
            {
                    m_collisionCategories = CollisionCategories.Geom;
                    if (m_isphysical)
                        m_collisionCategories |= CollisionCategories.Body;

                    m_collisionFlags = m_default_collisionFlags;

                    if (m_collidesLand)
                        m_collisionFlags |= CollisionCategories.Land;
                    if (m_collidesWater)
                        m_collisionFlags |= CollisionCategories.Water;

                if (prim_geom != IntPtr.Zero)
                {
                    if (m_NoColide)
                    {
                        d.GeomSetCategoryBits(prim_geom, 0);
                        if (m_isphysical)
                            d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
                        else
                        {
                            d.GeomSetCollideBits(prim_geom, 0);
                            d.GeomDisable(prim_geom);
                        }
                    }
                    else
                    {
                        d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                        d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                    }
                }
                if (Body != IntPtr.Zero)
                {
                    d.BodySetLinearVel(Body, 0f, 0f, 0f);
                    d.BodySetForce(Body, 0f, 0f, 0f);
                    d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
                    d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
                }

                if (m_isphysical)
                {
                    if (Body != IntPtr.Zero)
                    {
                        enableBodySoft();
                    }
                }
            }

            resetCollisionAccounting();
            m_isSelected = m_taintselected;
        }//end changeSelectedStatus

        public void ResetTaints()
        {
            m_taintposition = _position;
            m_taintrot = _orientation;
            m_taintPhysics = m_isphysical;
            m_taintselected = m_isSelected;
            m_taintsize = _size;
            m_taintshape = false;
            m_taintforce = false;
            m_taintdisable = false;
            m_taintVelocity = Vector3.Zero;
        }

        public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
        {
            bool gottrimesh = false;

            m_NoColide = false; // assume all will go well

            if (_triMeshData != IntPtr.Zero)
            {
                d.GeomTriMeshDataDestroy(_triMeshData);
                _triMeshData = IntPtr.Zero;
            }

            if (_mesh != null)
            {
                gottrimesh = setMesh(_parent_scene, _mesh);
                if (!gottrimesh)
                {
                    // getting a mesh failed,
                    // lets go on having a basic box or sphere, with prim size but not coliding
                    // physical colides with land, non with nothing

                    m_NoColide = true;
                }
            }

            if (!gottrimesh)
            { // we will have a basic box or sphere
                IntPtr geo = IntPtr.Zero;

                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
                    && _size.X == _size.Y && _size.X == _size.Z)
                {
                    // its a sphere
                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
                    try
                    {
                        geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
                    }
                    catch (Exception e)
                    {
                        m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
                        geo = IntPtr.Zero;
                        ode.dunlock(_parent_scene.world);
                    }
                }
                else // make it a box
                {
                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
                    try
                    {
                        geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
                    }
                    catch (Exception e)
                    {
                        m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
                        geo = IntPtr.Zero;
                        ode.dunlock(_parent_scene.world);
                    }
                }

                if (geo == IntPtr.Zero) // if this happens it must be fixed
                {
                    // if it does lets stop what we can
                    // not sure this will not flame...

                    m_taintremove = true;
                    _parent_scene.AddPhysicsActorTaint(this);
                    return;
                }

                SetGeom(geo); // this processes the m_NoColide
            }
        }

        public void changeadd(float timestep)
        {
            int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);

            if (targetspace == IntPtr.Zero)
                targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);

            m_targetSpace = targetspace;

            if (_mesh == null && m_meshfailed == false)
            {
                if (_parent_scene.needsMeshing(_pbs))
                {
                    try
                    {
                        _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true);
                    }
                    catch
                    {
                        //Don't continuously try to mesh prims when meshing has failed
                        m_meshfailed = true;
                        _mesh = null;
                        m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                    }
                }
            }

            lock (_parent_scene.OdeLock)
            {
                CreateGeom(m_targetSpace, _mesh);

                if (prim_geom != IntPtr.Zero)
                {
                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                    d.Quaternion myrot = new d.Quaternion();
                    myrot.X = _orientation.X;
                    myrot.Y = _orientation.Y;
                    myrot.Z = _orientation.Z;
                    myrot.W = _orientation.W;
                    d.GeomSetQuaternion(prim_geom, ref myrot);
                }

                if (m_isphysical && Body == IntPtr.Zero)
                {
                    enableBody();
                }
            }

            changeSelectedStatus(timestep);

            m_taintadd = false;
        }

        public void changemove(float timestep)
        {
            if (m_isphysical)
            {
                //                if (!m_disabled && !m_taintremove && !childPrim)  After one edit m_disabled is sometimes set, disabling further edits!
                if (!m_taintremove && !childPrim)
                {
                    if (Body == IntPtr.Zero)
                        enableBody();
                    //Prim auto disable after 20 frames,
                    //if you move it, re-enable the prim manually.
                    if (_parent != null)
                    {
                        if (m_linkJoint != IntPtr.Zero)
                        {
                            d.JointDestroy(m_linkJoint);
                            m_linkJoint = IntPtr.Zero;
                        }
                    }
                    if (Body != IntPtr.Zero)
                    {
                        d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);

                        if (_parent != null)
                        {
                            OdePrim odParent = (OdePrim)_parent;
                            if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
                            {
                                // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
                                Console.WriteLine("ODEPrim JointCreateFixed !!!");
                                m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
                                d.JointAttach(m_linkJoint, Body, odParent.Body);
                                d.JointSetFixed(m_linkJoint);
                            }
                        }
                        d.BodyEnable(Body);
                        if (m_type != Vehicle.TYPE_NONE)
                        {
                            Enable(Body, _parent_scene);
                        }
                    }
                    else
                    {
                        m_log.Warn("[PHYSICS]: Body Still null after enableBody().  This is a crash scenario.");
                    }
                }
                //else
                // {
                //m_log.Debug("[BUG]: race!");
                //}
            }
            else
            {
                // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
                // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
                _parent_scene.waitForSpaceUnlock(m_targetSpace);

                IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
                m_targetSpace = tempspace;

                _parent_scene.waitForSpaceUnlock(m_targetSpace);
                if (prim_geom != IntPtr.Zero)
                {
                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);

                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
                    d.SpaceAdd(m_targetSpace, prim_geom);
                }
            }

            changeSelectedStatus(timestep);

            resetCollisionAccounting();
            m_taintposition = _position;
        }

        public void 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;
        }

        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)
                    {
                        _mesh = null;
                        changeadd(2f);
                    }
                    if (childPrim)
                    {
                        if (_parent != null)
                        {
                            OdePrim parent = (OdePrim)_parent;
                            parent.ChildDelink(this);
                        }
                    }
                    else
                    {
                        disableBody();
                    }
                }
            }

            changeSelectedStatus(timestep);

            resetCollisionAccounting();
            m_taintPhysics = m_isphysical;
        }

        public void changesize(float timestamp)
        {

            string oldname = _parent_scene.geom_name_map[prim_geom];

            if (_size.X <= 0) _size.X = 0.01f;
            if (_size.Y <= 0) _size.Y = 0.01f;
            if (_size.Z <= 0) _size.Z = 0.01f;

            // Cleanup of old prim geometry
            if (_mesh != null)
            {
                // Cleanup meshing here
            }
            //kill body to rebuild
            if (IsPhysical && Body != IntPtr.Zero)
            {
                if (childPrim)
                {
                    if (_parent != null)
                    {
                        OdePrim parent = (OdePrim)_parent;
                        parent.ChildDelink(this);
                    }
                }
                else
                {
                    disableBody();
                }
            }
            if (d.SpaceQuery(m_targetSpace, prim_geom))
            {
                _parent_scene.waitForSpaceUnlock(m_targetSpace);
                d.SpaceRemove(m_targetSpace, prim_geom);
            }
            // we don't need to do space calculation because the client sends a position update also.

            // Construction of new prim
            if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false)
            {
                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;

                try
                {
                    if (_parent_scene.needsMeshing(_pbs))
                        mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
                }
                catch
                {
                    m_meshfailed = true;
                    mesh = null;
                    m_log.WarnFormat("[PHYSICS]: changeSize CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                }

                //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
                CreateGeom(m_targetSpace, mesh);
            }
            else
            {
                _mesh = null;
                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();
                }
            }


            // 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) && m_meshfailed == false)
            {
                // Don't need to re-enable body..   it's done in SetMesh
                float meshlod = _parent_scene.meshSculptLOD;
                IMesh mesh;

                if (IsPhysical)
                    meshlod = _parent_scene.MeshSculptphysicalLOD;
                try
                {
                    mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
                }
                catch
                {
                    mesh = null;
                    m_meshfailed = true;
                    m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                }

                CreateGeom(m_targetSpace, mesh);

                // createmesh returns null when it doesn't mesh.
            }
            else
            {
                _mesh = null;
                CreateGeom(m_targetSpace, null);
            }

            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
            d.Quaternion myrot = new d.Quaternion();
            //myrot.W = _orientation.w;
            myrot.W = _orientation.W;
            myrot.X = _orientation.X;
            myrot.Y = _orientation.Y;
            myrot.Z = _orientation.Z;
            d.GeomSetQuaternion(prim_geom, ref myrot);

            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
            if (IsPhysical && Body == IntPtr.Zero)
            {
                // Re creates body on size.
                // EnableBody also does setMass()
                enableBody();
                if (Body != IntPtr.Zero)
                {
                    d.BodyEnable(Body);
                }
            }
            _parent_scene.geom_name_map[prim_geom] = oldname;

            changeSelectedStatus(timestamp);
            if (childPrim)
            {
                if (_parent is OdePrim)
                {
                    OdePrim parent = (OdePrim)_parent;
                    parent.ChildSetGeom(this);
                }
            }
            resetCollisionAccounting();
            m_taintshape = false;
        }

        public void changeAddForce(float timestamp)
        {
            if (!m_isSelected)
            {
                lock (m_forcelist)
                {
                    //m_log.Info("[PHYSICS]: dequeing forcelist");
                    if (IsPhysical)
                    {
                        Vector3 iforce = Vector3.Zero;
                        int i = 0;
                        try
                        {
                            for (i = 0; i < m_forcelist.Count; i++)
                            {

                                iforce = iforce + (m_forcelist[i] * 100);
                            }
                        }
                        catch (IndexOutOfRangeException)
                        {
                            m_forcelist = new List<Vector3>();
                            m_collisionscore = 0;
                            m_interpenetrationcount = 0;
                            m_taintforce = false;
                            return;
                        }
                        catch (ArgumentOutOfRangeException)
                        {
                            m_forcelist = new List<Vector3>();
                            m_collisionscore = 0;
                            m_interpenetrationcount = 0;
                            m_taintforce = false;
                            return;
                        }
                        d.BodyEnable(Body);

                        d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
                    }
                    m_forcelist.Clear();
                }

                m_collisionscore = 0;
                m_interpenetrationcount = 0;
            }

            m_taintforce = false;

        }



        public void changeSetTorque(float timestamp)
        {
            if (!m_isSelected)
            {
                if (IsPhysical && Body != IntPtr.Zero)
                {
                    d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
                }
            }

            m_taintTorque = Vector3.Zero;
        }

        public void changeAddAngularForce(float timestamp)
        {
            if (!m_isSelected)
            {
                lock (m_angularforcelist)
                {
                    //m_log.Info("[PHYSICS]: dequeing forcelist");
                    if (IsPhysical)
                    {
                        Vector3 iforce = Vector3.Zero;
                        for (int i = 0; i < m_angularforcelist.Count; i++)
                        {
                            iforce = iforce + (m_angularforcelist[i] * 100);
                        }
                        d.BodyEnable(Body);
                        d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);

                    }
                    m_angularforcelist.Clear();
                }

                m_collisionscore = 0;
                m_interpenetrationcount = 0;
            }

            m_taintaddangularforce = false;
        }

        private void changevelocity(float timestep)
        {
            if (!m_isSelected)
            {
                Thread.Sleep(20);
                if (IsPhysical)
                {
                    if (Body != IntPtr.Zero)
                        d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
                }

                //resetCollisionAccounting();
            }
            m_taintVelocity = Vector3.Zero;
        }

        public void UpdatePositionAndVelocity()
        {
            return;   // moved to the Move () method
        }

        public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
        {
            obj.I.M00 = pMat[0, 0];
            obj.I.M01 = pMat[0, 1];
            obj.I.M02 = pMat[0, 2];
            obj.I.M10 = pMat[1, 0];
            obj.I.M11 = pMat[1, 1];
            obj.I.M12 = pMat[1, 2];
            obj.I.M20 = pMat[2, 0];
            obj.I.M21 = pMat[2, 1];
            obj.I.M22 = pMat[2, 2];
            return obj;
        }

        public override void SubscribeEvents(int ms)
        {
            m_eventsubscription = ms;
            _parent_scene.addCollisionEventReporting(this);
        }

        public override void UnSubscribeEvents()
        {
            _parent_scene.remCollisionEventReporting(this);
            m_eventsubscription = 0;
        }

        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
        {
            if (CollisionEventsThisFrame == null)
                CollisionEventsThisFrame = new CollisionEventUpdate();
            CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
        }

        public void SendCollisions()
        {
            if (CollisionEventsThisFrame == null)
                return;

            base.SendCollisionUpdate(CollisionEventsThisFrame);

            if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
                CollisionEventsThisFrame = null;
            else
                CollisionEventsThisFrame = new CollisionEventUpdate();
        }

        public override bool SubscribedEvents()
        {
            if (m_eventsubscription > 0)
                return true;
            return false;
        }

        public static Matrix4 Inverse(Matrix4 pMat)
        {
            if (determinant3x3(pMat) == 0)
            {
                return Matrix4.Identity; // should probably throw an error.  singluar matrix inverse not possible
            }



            return (Adjoint(pMat) / determinant3x3(pMat));
        }

        public static Matrix4 Adjoint(Matrix4 pMat)
        {
            Matrix4 adjointMatrix = new Matrix4();
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
                }
            }

            adjointMatrix = Transpose(adjointMatrix);
            return adjointMatrix;
        }

        public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
        {
            Matrix4 minor = new Matrix4();
            int m = 0, n = 0;
            for (int i = 0; i < 4; i++)
            {
                if (i == iRow)
                    continue;
                n = 0;
                for (int j = 0; j < 4; j++)
                {
                    if (j == iCol)
                        continue;
                    Matrix4SetValue(ref minor, m, n, matrix[i, j]);
                    n++;
                }
                m++;
            }
            return minor;
        }

        public static Matrix4 Transpose(Matrix4 pMat)
        {
            Matrix4 transposeMatrix = new Matrix4();
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
            return transposeMatrix;
        }

        public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
        {
            switch (r)
            {
                case 0:
                    switch (c)
                    {
                        case 0:
                            pMat.M11 = val;
                            break;
                        case 1:
                            pMat.M12 = val;
                            break;
                        case 2:
                            pMat.M13 = val;
                            break;
                        case 3:
                            pMat.M14 = val;
                            break;
                    }

                    break;
                case 1:
                    switch (c)
                    {
                        case 0:
                            pMat.M21 = val;
                            break;
                        case 1:
                            pMat.M22 = val;
                            break;
                        case 2:
                            pMat.M23 = val;
                            break;
                        case 3:
                            pMat.M24 = val;
                            break;
                    }

                    break;
                case 2:
                    switch (c)
                    {
                        case 0:
                            pMat.M31 = val;
                            break;
                        case 1:
                            pMat.M32 = val;
                            break;
                        case 2:
                            pMat.M33 = val;
                            break;
                        case 3:
                            pMat.M34 = val;
                            break;
                    }

                    break;
                case 3:
                    switch (c)
                    {
                        case 0:
                            pMat.M41 = val;
                            break;
                        case 1:
                            pMat.M42 = val;
                            break;
                        case 2:
                            pMat.M43 = val;
                            break;
                        case 3:
                            pMat.M44 = val;
                            break;
                    }

                    break;
            }
        }
        private static float determinant3x3(Matrix4 pMat)
        {
            float det = 0;
            float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2];
            float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0];
            float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1];
            float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2];
            float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0];
            float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1];

            det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
            return det;

        }

        private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
        {
            dst.c.W = src.c.W;
            dst.c.X = src.c.X;
            dst.c.Y = src.c.Y;
            dst.c.Z = src.c.Z;
            dst.mass = src.mass;
            dst.I.M00 = src.I.M00;
            dst.I.M01 = src.I.M01;
            dst.I.M02 = src.I.M02;
            dst.I.M10 = src.I.M10;
            dst.I.M11 = src.I.M11;
            dst.I.M12 = src.I.M12;
            dst.I.M20 = src.I.M20;
            dst.I.M21 = src.I.M21;
            dst.I.M22 = src.I.M22;
        }

        public override void SetMaterial(int pMaterial)
        {
            m_material = pMaterial;
        }

        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.1f) pValue = 0.1f;
                    // m_angularDeflectionTimescale = pValue;
                    break;
                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
                    if (pValue < 0.3f) pValue = 0.3f;
                    m_angularMotorDecayTimescale = pValue;
                    break;
                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
                    if (pValue < 0.3f) pValue = 0.3f;
                    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.1f) pValue = 0.1f;
                    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.3f) pValue = 0.3f;
                    m_linearMotorDecayTimescale = pValue;
                    break;
                case Vehicle.LINEAR_MOTOR_TIMESCALE:
                    if (pValue < 0.1f) pValue = 0.1f;
                    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.1f) pValue = 0.1f;
                    m_verticalAttractionTimescale = pValue;
                    break;

                // These are vector properties but the engine lets you use a single float value to 
                // set all of the components to the same value
                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
                    if (pValue > 30f) pValue = 30f;
                    if (pValue < 0.1f) pValue = 0.1f;
                    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:
                    if (pValue < 0.1f) pValue = 0.1f;
                    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:
                    if (pValue.X < 0.1f) pValue.X = 0.1f;
                    if (pValue.Y < 0.1f) pValue.Y = 0.1f;
                    if (pValue.Z < 0.1f) pValue.Z = 0.1f;
                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
                    break;
                case Vehicle.LINEAR_MOTOR_DIRECTION:
                    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 ProcessVehicleFlags(int pParam, bool remove)
        {
            if (remove)
            {
                m_flags &= ~((VehicleFlag)pParam);
            }
            else
            {
                m_flags |= (VehicleFlag)pParam;
            }
        }

        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 Halt()
        {	// Kill all motions, when non-physical
            //	m_linearMotorDirection = Vector3.Zero;
            m_lLinMotorDVel = Vector3.Zero;
            m_lLinObjectVel = Vector3.Zero;
            m_wLinObjectVel = Vector3.Zero;
            m_angularMotorDirection = Vector3.Zero;
            m_lastAngularVelocity = Vector3.Zero;
            m_angularMotorDVel = Vector3.Zero;
            _acceleration = Vector3.Zero;
        }

        private void UpdateLinDecay()
        {
            m_lLinMotorDVel.X = m_linearMotorDirection.X;
            m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
            m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
        } // else let the motor decay on its own

        private void UpdateAngDecay()
        {
            m_angularMotorDVel.X = m_angularMotorDirection.X;
            m_angularMotorDVel.Y = m_angularMotorDirection.Y;
            m_angularMotorDVel.Z = m_angularMotorDirection.Z;
        } // else let the motor decay on its own

        public void Move(float timestep)
        {
            float fx = 0;
            float fy = 0;
            float fz = 0;
            Vector3 linvel;				// velocity applied, including any reversal

            // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
            // This is a temp patch until proper region crossing is developed. 


            if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds)		// Only move root prims.
            {
                //  Old public void UpdatePositionAndVelocity(), more accuratley calculated here
                bool lastZeroFlag = _zeroFlag;  // was it stopped

                d.Vector3 vec = d.BodyGetPosition(Body);
                Vector3 l_position = Vector3.Zero;
                l_position.X = vec.X;
                l_position.Y = vec.Y;
                l_position.Z = vec.Z;
                m_lastposition = _position;
                _position = l_position;

                d.Quaternion ori = d.BodyGetQuaternion(Body);
                //       Quaternion l_orientation = Quaternion.Identity;
                _orientation.X = ori.X;
                _orientation.Y = ori.Y;
                _orientation.Z = ori.Z;
                _orientation.W = ori.W;
                m_lastorientation = _orientation;

                d.Vector3 vel = d.BodyGetLinearVel(Body);
                m_lastVelocity = _velocity;
                _velocity.X = vel.X;
                _velocity.Y = vel.Y;
                _velocity.Z = vel.Z;
                _acceleration = ((_velocity - m_lastVelocity) / timestep);

                d.Vector3 torque = d.BodyGetTorque(Body);
                _torque = new Vector3(torque.X, torque.Y, torque.Z);


                if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X
                    || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y
                    )
                {
                    // we are outside current region
                    // clip position to a stop just outside region and stop it only internally
                    // do it only once using m_crossingfailures as control
                    _position.X = Util.Clip(l_position.X, -0.2f, _parent_scene.WorldExtents.X + .2f);
                    _position.Y = Util.Clip(l_position.Y, -0.2f, _parent_scene.WorldExtents.Y + .2f);
                    _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
                    d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
                    d.BodySetLinearVel(Body, 0, 0, 0);
                    m_outofBounds = true;
                    base.RequestPhysicsterseUpdate();
                    return;
                }

                base.RequestPhysicsterseUpdate();

                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;			// This stuff may stop client display but it has no
                    _acceleration.Y = 0;			// effect on the object in phys engine!
                    _acceleration.Z = 0;

                    _velocity.X = 0;
                    _velocity.Y = 0;
                    _velocity.Z = 0;
                    m_lastVelocity = Vector3.Zero;
                    m_rotationalVelocity.X = 0;
                    m_rotationalVelocity.Y = 0;
                    m_rotationalVelocity.Z = 0;

                    if (_parent == null) base.RequestPhysicsterseUpdate();

                    m_throttleUpdates = false;
                    throttleCounter = 0;
                    _zeroFlag = true;
                    //outofBounds = true;
                }  // end neg Z check

                // Is it moving?
                /*   if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
                       && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
                       && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
                if ((Vector3.Mag(_velocity) < 0.01) &&                         // moving very slowly
                     (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) &&  // decelerating
                     (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001))  // spinning very slowly
                {
                    _zeroFlag = true;
                    m_throttleUpdates = false;
                }
                else
                {
                    //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;

                    m_rotationalVelocity.X = 0;
                    m_rotationalVelocity.Y = 0;
                    m_rotationalVelocity.Z = 0;
                    // Stop it in the phys engine
                    d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
                    d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
                    d.BodySetForce(Body, 0f, 0f, 0f);

                    if (!m_lastUpdateSent)
                    {
                        m_throttleUpdates = false;
                        throttleCounter = 0;
                        if (_parent == null)
                        {
                            base.RequestPhysicsterseUpdate();
                        }

                        m_lastUpdateSent = true;
                    }
                }
                else
                {			// Its moving
                    if (lastZeroFlag != _zeroFlag)
                    {
                        if (_parent == null)
                        {
                            base.RequestPhysicsterseUpdate();
                        }
                    }
                    m_lastUpdateSent = false;
                    if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
                    {
                        if (_parent == null)
                        {
                            base.RequestPhysicsterseUpdate();
                        }
                    }
                    else
                    {
                        throttleCounter++;
                    }
                }
                m_lastposition = l_position;

                /// End UpdatePositionAndVelocity insert     


                // Rotation lock  =====================================
                if (m_rotateEnableUpdate)
                {
                    // Snapshot current angles, set up Amotor(s)
                    m_rotateEnableUpdate = false;
                    m_rotateEnable = m_rotateEnableRequest;
                    //Console.WriteLine("RotEnable {0} = {1}",m_primName,  m_rotateEnable);

                    if (Amotor != IntPtr.Zero)
                    {
                        d.JointDestroy(Amotor);
                        Amotor = IntPtr.Zero;
                        //Console.WriteLine("Old Amotor Destroyed");                        
                    }

                    if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
                    {   // not all are enabled
                        d.Quaternion r = d.BodyGetQuaternion(Body);
                        Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
                        // extract the axes vectors
                        Vector3 vX = new Vector3(1f, 0f, 0f);
                        Vector3 vY = new Vector3(0f, 1f, 0f);
                        Vector3 vZ = new Vector3(0f, 0f, 1f);
                        vX = vX * locrot;
                        vY = vY * locrot;
                        vZ = vZ * locrot;
                        // snapshot the current angle vectors
                        m_lockX = vX;
                        m_lockY = vY;
                        m_lockZ = vZ;
                        //           m_lockRot = locrot;
                        Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
                        d.JointAttach(Amotor, Body, IntPtr.Zero);
                        d.JointSetAMotorMode(Amotor, 0);  // User mode??
                        //Console.WriteLine("New Amotor Created for {0}", m_primName);                   

                        float axisnum = 3;  // how many to lock
                        axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
                        d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
                        //Console.WriteLine("AxisNum={0}",(int)axisnum);                       

                        int i = 0;

                        if (m_rotateEnable.X == 0)
                        {
                            d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
                            //Console.WriteLine("AxisX {0} set to {1}", i,  m_lockX);                           
                            i++;
                        }

                        if (m_rotateEnable.Y == 0)
                        {
                            d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
                            //Console.WriteLine("AxisY {0} set to {1}", i,  m_lockY);                           
                            i++;
                        }

                        if (m_rotateEnable.Z == 0)
                        {
                            d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
                            //Console.WriteLine("AxisZ {0} set to {1}", i,  m_lockZ);                           
                            i++;
                        }

                        // These lowstops and high stops are effectively (no wiggle room)
                        d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
                        d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
                    } // else none are locked
                } // end Rotation Update  


                // VEHICLE processing ==========================================            
                if (m_type != Vehicle.TYPE_NONE)
                {
                    // get body attitude
                    d.Quaternion rot = d.BodyGetQuaternion(Body);
                    Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);	// rotq = rotation of object
                    Quaternion irotq = Quaternion.Inverse(rotq);

                    // VEHICLE Linear Motion
                    d.Vector3 velnow = d.BodyGetLinearVel(Body);					// this is in world frame
                    Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
                    m_lLinObjectVel = vel_now * irotq;
                    if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
                    {
                        if (Vector3.Mag(m_lLinMotorDVel) < 1.0f)
                        {
                            float decayfactor = m_linearMotorDecayTimescale / timestep;
                            Vector3 decayAmount = (m_lLinMotorDVel / decayfactor);
                            m_lLinMotorDVel -= decayAmount;
                        }
                        else
                        {
                            float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
                            Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
                            m_lLinMotorDVel -= decel;
                        }
                        if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
                        {
                            m_lLinMotorDVel = Vector3.Zero;
                        }

                        /*	else
                            {
                                if (Math.Abs(m_lLinMotorDVel.X) <  Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
                                if (Math.Abs(m_lLinMotorDVel.Y) <  Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
                                if (Math.Abs(m_lLinMotorDVel.Z) <  Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;	
                            } */
                    }  // end linear motor decay

                    if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
                    {
                        if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
                        if (m_linearMotorTimescale < 300.0f)
                        {
                            Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
                            float linfactor = m_linearMotorTimescale / timestep;
                            Vector3 attackAmount = (attack_error / linfactor) * 1.3f;
                            m_lLinObjectVel += attackAmount;
                        }
                        if (m_linearFrictionTimescale.X < 300.0f)
                        {
                            float fricfactor = m_linearFrictionTimescale.X / timestep;
                            float fricX = m_lLinObjectVel.X / fricfactor;
                            m_lLinObjectVel.X -= fricX;
                        }
                        if (m_linearFrictionTimescale.Y < 300.0f)
                        {
                            float fricfactor = m_linearFrictionTimescale.Y / timestep;
                            float fricY = m_lLinObjectVel.Y / fricfactor;
                            m_lLinObjectVel.Y -= fricY;
                        }
                        if (m_linearFrictionTimescale.Z < 300.0f)
                        {
                            float fricfactor = m_linearFrictionTimescale.Z / timestep;
                            float fricZ = m_lLinObjectVel.Z / fricfactor;
                            m_lLinObjectVel.Z -= fricZ;
                        }
                    }
                    m_wLinObjectVel = m_lLinObjectVel * rotq;

                    // Gravity and Buoyancy
                    Vector3 grav = Vector3.Zero;
                    if (m_VehicleBuoyancy < 1.0f)
                    {
                        // There is some gravity, make a gravity force vector
                        // that is applied after object velocity.     
                        d.Mass objMass;
                        d.BodyGetMass(Body, out objMass);
                        // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 
                        grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
                    } // else its 1.0, no gravity.

                    // Hovering
                    if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
                    {
                        // We should hover, get the target height
                        d.Vector3 pos = d.BodyGetPosition(Body);
                        if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
                        {
                            m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
                        }
                        else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
                        {
                            m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                        }
                        else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
                        {
                            m_VhoverTargetHeight = m_VhoverHeight;
                        }

                        if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
                        {
                            // If body is aready heigher, use its height as target height
                            if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
                        }

                        //	            m_VhoverEfficiency = 0f;	// 0=boucy, 1=Crit.damped
                        //				m_VhoverTimescale = 0f;		// time to acheive height
                        //				timestep  is time since last frame,in secs 
                        float herr0 = pos.Z - m_VhoverTargetHeight;
                        // Replace Vertical speed with correction figure if significant
                        if (Math.Abs(herr0) > 0.01f)
                        {
                            //?          d.Mass objMass;
                            //?          d.BodyGetMass(Body, out objMass);
                            m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale);
                            //KF: m_VhoverEfficiency is not yet implemented
                        }
                        else
                        {
                            m_wLinObjectVel.Z = 0f;
                        }
                    }
                    else
                    {	// not hovering
                        if (m_wLinObjectVel.Z == 0f)
                        {		// Gravity rules
                            m_wLinObjectVel.Z = vel_now.Z;
                        }  // else the motor has it
                    }
                    linvel = m_wLinObjectVel;

                    // Vehicle Linear  Motion done =======================================
                    // Apply velocity
                    d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
                    // apply gravity force
                    d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
                    //if(frcount == 0) Console.WriteLine("Vel={0}    Force={1}",linvel ,  grav);
                    // end MoveLinear()


                    // MoveAngular
                    /*
                    private Vector3 m_angularMotorDirection = Vector3.Zero;			// angular velocity requested by LSL motor 
        
                    private float m_angularMotorTimescale = 0;						// motor angular Attack rate set by LSL
                    private float m_angularMotorDecayTimescale = 0;					// motor angular Decay rate set by LSL
                    private Vector3 m_angularFrictionTimescale = Vector3.Zero;		// body angular Friction set by LSL

                    private Vector3 m_angularMotorDVel = Vector3.Zero;				// decayed angular motor
                    private Vector3 m_angObjectVel = Vector3.Zero;					// what was last applied to body
                    */
                    //if(frcount == 0) Console.WriteLine("MoveAngular ");	

                    d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
                    Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
                    angObjectVel = angObjectVel * irotq;	// ============ Converts to LOCAL rotation

                    //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);        	

                    // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
                    float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
                    m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
                    // Decay Angular Motor 2.
                    if (m_angularMotorDecayTimescale < 300.0f)
                    {
                        if (Vector3.Mag(m_angularMotorDVel) < 1.0f)
                        {
                            float decayfactor = (m_angularMotorDecayTimescale) / timestep;
                            Vector3 decayAmount = (m_angularMotorDVel / decayfactor);
                            m_angularMotorDVel -= decayAmount;
                        }
                        else
                        {
                            Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
                            m_angularMotorDVel -= decel;
                        }

                        if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
                        {
                            m_angularMotorDVel = Vector3.Zero;
                        }
                        else
                        {
                            if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
                            if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
                            if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
                        }
                    } // end decay angular motor
                    //if(frcount == 0) Console.WriteLine("MotorDvel {0}    Obj {1}", m_angularMotorDVel, angObjectVel);

                    //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);   

                    if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
                    {  // if motor or object have motion
                        if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);

                        if (m_angularMotorTimescale < 300.0f)
                        {
                            Vector3 attack_error = m_angularMotorDVel - angObjectVel;
                            float angfactor = m_angularMotorTimescale / timestep;
                            Vector3 attackAmount = (attack_error / angfactor);
                            angObjectVel += attackAmount;
                            //if(frcount == 0) Console.WriteLine("Accel {0}      Attk {1}",FrAaccel, attackAmount);                	
                            //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);        	
                        }

                        angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
                        angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
                        angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
                    } // else no signif. motion

                    //if(frcount == 0) Console.WriteLine("Dmotor {0}      Obj {1}", m_angularMotorDVel, angObjectVel);
                    // Bank section tba
                    // Deflection section tba
                    //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);        	


                    /*			// Rotation Axis Disables:
                                if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
                                {
                                    if (m_angularEnable.X == 0)
                                        angObjectVel.X = 0f;
                                    if (m_angularEnable.Y == 0)
                                        angObjectVel.Y = 0f;
                                    if (m_angularEnable.Z == 0)
                                        angObjectVel.Z = 0f;
                                }        
                        */
                    angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation

                    // Vertical attractor section
                    Vector3 vertattr = Vector3.Zero;

                    if (m_verticalAttractionTimescale < 300)
                    {
                        float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
                        // make a vector pointing up
                        Vector3 verterr = Vector3.Zero;
                        verterr.Z = 1.0f;
                        // rotate it to Body Angle
                        verterr = verterr * rotq;
                        // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
                        // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
                        // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.

                        if (verterr.Z < 0.0f)
                        {	// Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
                            // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
                            //Console.WriteLine("InvertFlip");	
                            verterr.X = 2.0f - verterr.X;
                            verterr.Y = 2.0f - verterr.Y;
                        }
                        verterr *= 0.5f;
                        // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
                        Vector3 xyav = angObjectVel;
                        xyav.Z = 0.0f;
                        if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
                        {
                            // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 
                            // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
                            vertattr.X = verterr.Y;
                            vertattr.Y = -verterr.X;
                            vertattr.Z = 0f;
                            //if(frcount == 0) Console.WriteLine("VAerr=" + verterr);	

                            // scaling appears better usingsquare-law
                            float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
                            float bounce = 1.0f - damped;
                            // 0 = crit damp, 1 = bouncy
                            float oavz = angObjectVel.Z;   // retain z velocity
                            // time-scaled correction, which sums, therefore is bouncy:
                            angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
                            // damped, good @ < 90:
                            angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
                            angObjectVel.Z = oavz;
                            //if(frcount == 0) Console.WriteLine("VA+");					
                            //Console.WriteLine("VAttr {0}         OAvel {1}", vertattr, angObjectVel);
                        }
                        else
                        {
                            // else error is very small
                            angObjectVel.X = 0f;
                            angObjectVel.Y = 0f;
                            //if(frcount == 0) Console.WriteLine("VA0");					
                        }
                    } // else vertical attractor is off
                    //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);        	


                    m_lastAngularVelocity = angObjectVel;
                    // apply Angular Velocity to body
                    d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
                    //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);        	

                }  // end VEHICLES
                else
                {
                    // Dyamics (NON-'VEHICLES') are dealt with here ================================================================	                

                    if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009

                    /// Dynamics Buoyancy
                    //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
                    // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
                    // NB Prims in ODE are no subject to global gravity
                    // This should only affect gravity operations 

                    float m_mass = CalculateMass();
                    // calculate z-force due togravity on object.
                    fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
                    if ((m_usePID) && (m_PIDTau > 0.0f))  // Dynamics  llMoveToTarget.
                    {
                        fz = 0;     // llMoveToTarget ignores gravity.
                        // it also ignores mass of object, and any physical resting on it.
                        // Vector3 m_PIDTarget is where we are going
                        // float m_PIDTau is time to get there
                        fx = 0;
                        fy = 0;
                        d.Vector3 pos = d.BodyGetPosition(Body);
                        Vector3 error = new Vector3(
                                (m_PIDTarget.X - pos.X),
                                (m_PIDTarget.Y - pos.Y),
                                (m_PIDTarget.Z - pos.Z));
                        if (error.ApproxEquals(Vector3.Zero, 0.01f))
                        {   // Very close, Jump there and quit move

                            d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
                            _target_velocity = Vector3.Zero;
                            d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
                            d.BodySetForce(Body, 0f, 0f, 0f);
                        }
                        else
                        {
                            float scale = 50.0f * timestep / m_PIDTau;
                            if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero))
                            {
                                // Nearby, quit update of velocity
                            }
                            else
                            {  // Far, calc damped velocity
                                _target_velocity = error * scale;
                            }
                            d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
                        }
                    } // end PID MoveToTarget


                    /// Dynamics Hover ===================================================================================
                    // Hover PID Controller can only run if the PIDcontroller is not in use.
                    if (m_useHoverPID && !m_usePID)
                    {
                        //Console.WriteLine("Hover " +  m_primName);           	

                        // If we're using the PID controller, then we have no gravity
                        fz = (-1 * _parent_scene.gravityz) * m_mass;

                        //  no lock; for now it's only called from within Simulate()

                        // If the PID Controller isn't active then we set our force
                        // calculating base velocity to the current position

                        if ((m_PIDTau < 1))
                        {
                            PID_G = PID_G / m_PIDTau;
                        }

                        if ((PID_G - m_PIDTau) <= 0)
                        {
                            PID_G = m_PIDTau + 1;
                        }


                        // Where are we, and where are we headed?
                        d.Vector3 pos = d.BodyGetPosition(Body);
                        //	                    d.Vector3 vel = d.BodyGetLinearVel(Body);


                        //    Non-Vehicles have a limited set of Hover options.
                        // determine what our target height really is based on HoverType
                        switch (m_PIDHoverType)
                        {
                            case PIDHoverType.Ground:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                break;
                            case PIDHoverType.GroundAndWater:
                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
                                m_waterHeight = _parent_scene.GetWaterLevel();
                                if (m_groundHeight > m_waterHeight)
                                {
                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
                                }
                                else
                                {
                                    m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
                                }
                                break;

                        } 	// end switch (m_PIDHoverType)


                        _target_velocity =
                            new Vector3(0.0f, 0.0f,
                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
                                );

                        //  if velocity is zero, use position control; otherwise, velocity control

                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
                        {
                            //  keep track of where we stopped.  No more slippin' & slidin'

                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
                            // react to the physics scene by moving it's position.
                            // Avatar to Avatar collisions
                            // Prim to avatar collisions
                            d.Vector3 dlinvel = vel;
                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
                            d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
                            d.BodyAddForce(Body, 0, 0, fz);
                            //KF this prevents furthur motions         return;
                        }
                        else
                        {
                            _zeroFlag = false;

                            // We're flying and colliding with something
                            fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
                        }
                    } // end m_useHoverPID && !m_usePID


                    /// Dynamics Apply Forces ===================================================================================
                    fx *= m_mass;
                    fy *= m_mass;
                    //fz *= m_mass;
                    fx += m_force.X;
                    fy += m_force.Y;
                    fz += m_force.Z;

                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
                    if (fx != 0 || fy != 0 || fz != 0)
                    {
                        //m_taintdisable = true;
                        //base.RaiseOutOfBounds(Position);
                        //d.BodySetLinearVel(Body, fx, fy, 0f);
                        if (!d.BodyIsEnabled(Body))
                        {
                            // A physical body at rest on a surface will auto-disable after a while,
                            // this appears to re-enable it incase the surface it is upon vanishes,
                            // and the body should fall again. 
                            d.BodySetLinearVel(Body, 0f, 0f, 0f);
                            d.BodySetForce(Body, 0f, 0f, 0f);
                            enableBodySoft();
                        }

                        // 35x10 = 350n times the mass per second applied maximum.
                        float nmax = 35f * m_mass;
                        float nmin = -35f * m_mass;


                        if (fx > nmax)
                            fx = nmax;
                        if (fx < nmin)
                            fx = nmin;
                        if (fy > nmax)
                            fy = nmax;
                        if (fy < nmin)
                            fy = nmin;
                        d.BodyAddForce(Body, fx, fy, fz);
                    }  // end apply forces
                } // end Vehicle/Dynamics

                /// RotLookAt / LookAt =================================================================================
                if (m_useAPID)
                {
                    // RotLookAt, apparently overrides all other rotation sources. Inputs:
                    // Quaternion m_APIDTarget
                    // float m_APIDStrength		// From SL experiments, this is the time to get there
                    // float m_APIDDamping		// From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
                    // Also in SL the mass of the object has no effect on time to get there.
                    // Factors:
                    // get present body rotation
                    float limit = 1.0f;
                    float rscaler = 50f;		// adjusts rotation damping time
                    float lscaler = 10f;		// adjusts linear damping time in llLookAt
                    float RLAservo = 0f;
                    Vector3 diff_axis;
                    float diff_angle;
                    d.Quaternion rot = d.BodyGetQuaternion(Body);						// prim present rotation
                    Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
                    Quaternion rtarget = new Quaternion();

                    if (m_APIDTarget.W == -99.9f)
                    {
                        // this is really a llLookAt(), x,y,z is the target vector
                        Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
                        Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
                        Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
                        float twopi = 2.0f * (float)Math.PI;
                        Vector3 dir = target - _position;
                        dir.Normalize();
                        float tzrot = (float)Math.Atan2(dir.Y, dir.X);
                        float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
                        float terot = (float)Math.Atan2(dir.Z, txy);
                        float ozrot = (float)Math.Atan2(ospin.Y, ospin.X);
                        float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
                        float oerot = (float)Math.Atan2(ospin.Z, oxy);
                        float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
                        float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X));
                        float roll = (float)Math.Atan2(ra, rb);
                        float errorz = tzrot - ozrot;
                        if (errorz > (float)Math.PI) errorz -= twopi;
                        else if (errorz < -(float)Math.PI) errorz += twopi;
                        float errory = oerot - terot;
                        if (errory > (float)Math.PI) errory -= twopi;
                        else if (errory < -(float)Math.PI) errory += twopi;
                        diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
                        if (diff_angle > 0.01f * m_APIDdamper)
                        {
                            m_APIDdamper = 1.0f;
                            RLAservo = timestep / m_APIDStrength * rscaler;
                            errorz *= RLAservo;
                            errory *= RLAservo;
                            error.X = -roll * 8.0f;
                            error.Y = errory;
                            error.Z = errorz;
                            error *= rotq;
                            d.BodySetAngularVel(Body, error.X, error.Y, error.Z);
                        }
                        else
                        {
                            d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
                            m_APIDdamper = 2.0f;
                        }
                    }
                    else
                    {
                        // this is a llRotLookAt()
                        rtarget = m_APIDTarget;

                        Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget;		// difference to desired rot
                        rot_diff.GetAxisAngle(out diff_axis, out diff_angle);				// convert to axis to point at & error angle
                        //if(frcount == 0) Console.WriteLine("axis {0}   angle {1}",diff_axis * 57.3f, diff_angle);

                        //   diff_axis.Normalize(); it already is!
                        if (diff_angle > 0.01f * m_APIDdamper)			// diff_angle is always +ve			// if there is enough error
                        {
                            m_APIDdamper = 1.0f;
                            Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
                            rotforce = rotforce * rotq;
                            if (diff_angle > limit) diff_angle = limit;		// cap the rotate rate
                            RLAservo = timestep / m_APIDStrength * lscaler;
                            rotforce = rotforce * RLAservo * diff_angle;
                            d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z);
                            //Console.WriteLine("axis= " + diff_axis + "    angle= " + diff_angle + "servo= " + RLAservo);							
                        }
                        else
                        {	// close enough
                            d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
                            m_APIDdamper = 2.0f;
                        }
                    } // end llLookAt/llRotLookAt
                    //if(frcount == 0) Console.WriteLine("mass= " + m_mass + "  servo= " + RLAservo + "   angle= " + diff_angle);
                } // end m_useAPID
            } // end root prims
        }  // end Move()
    } // end class
}