/*

 * Copyright (c) Contributors, http://aurora-sim.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 Aurora-Sim 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 August 26 2009 by Kitto Flora. ODEDynamics.cs replaces

 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:

 * ODEPrim.cs contains methods dealing with Prim editing, Prim

 * characteristics and Kinetic motion.

 * ODEDynamics.cs contains methods dealing with Prim Physical motion

 * (dynamics) and the associated settings. Old Linear and angular

 * motors for dynamic motion have been replace with  MoveLinear()

 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 

 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to

 * switch between 'VEHICLE' parameter use and general dynamics

 * settings use.

 */



using Aurora.Framework;

using Aurora.Framework.ConsoleFramework;

using Aurora.Framework.Physics;

using Aurora.Framework.SceneInfo;

using Aurora.Framework.Utilities;

using OdeAPI;

using OpenMetaverse;

using System;

using System.Collections.Generic;

using System.Linq;



//using Ode.NET;



namespace Aurora.Physics.AuroraOpenDynamicsEngine

{

    /// <summary>

    ///     Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.

    /// </summary>

    public class AuroraODEPrim : PhysicsObject

    {

        private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom

                                                                      | CollisionCategories.Space

                                                                      | CollisionCategories.Body

                                                                      | CollisionCategories.Character

                                                                     );



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

        private readonly AuroraODEPhysicsScene _parent_scene;



        private readonly Vector3 _torque = Vector3.Zero;

        private readonly int body_autodisable_frames = 20;

        private readonly AuroraODEDynamics m_vehicle;

        private IntPtr Amotor = IntPtr.Zero;

        public IntPtr Body { get; private set; }

        private CollisionEventUpdate CollisionEventsThisFrame;



        private float PID_D = 35f;

        private float PID_G = 25f;

        private Vector3 _acceleration;

        private float _mass; // prim or object mass

        private IMesh _mesh;

        private Quaternion _orientation;

        private PhysicsObject _parent;

        private ISceneChildEntity _parent_entity;

        private PrimitiveBaseShape _pbs;

        private Vector3 _position;

        private Vector3 _size;

        private IntPtr _triMeshData;

        private Vector3 _velocity;

        private bool _zeroFlag;

        private bool _zeroFlagForceSet;

        internal volatile bool childPrim;

        internal List<AuroraODEPrim> childrenPrim = new List<AuroraODEPrim>();

        private int fakeori; // control the use of above

        private int fakepos; // control the use of above

        private bool hasOOBoffsetFromMesh; // if true we did compute it form mesh centroid, else from aabb

        private bool iscolliding;

        private Vector3 m_angularforceacc;

        private Vector3 m_angularlock = Vector3.One;

        private bool m_blockPhysicalReconstruction;

        private bool m_buildingRepresentation;



        // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),

        // and are for non-VEHICLES only.



        private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 



        // private float m_tensor = 5f;



        private bool m_collidesLand = true;

        private bool m_collidesWater;



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

        internal float m_collisionscore;

        private int m_crossingfailures;



        internal bool m_disabled;

        private bool m_eventsubscription;

        //This disables the prim so that it cannot do much anything at all



        private Vector3 m_force;

        private Vector3 m_forceacc;

        internal bool m_frozen;

        private bool m_isSelected;



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

        private bool m_isphysical;

        private int m_lastUpdateSent;

        private Vector3 m_lastVelocity;

        private Quaternion m_lastorientation;

        private Vector3 m_lastposition;

        private uint m_localID;

        private int m_material = (int) Material.Wood;

        private bool m_primIsRemoved;

        private Vector3 m_pushForce;

        private Vector3 m_rotationalVelocity;

        internal IntPtr m_targetSpace = IntPtr.Zero;



        private bool m_throttleUpdates;



        private float primMass; // prim own mass



        public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb

        public Vector3 primOOBsize; // prim real dimensions from mesh 

        public IntPtr prim_geom;

        private d.Mass primdMass; // prim inertia information on it's own referencial

        private Quaternion showorientation; // tmp hack see showposition

        private Vector3 showposition; // a temp hack for now rest of code expects position to be changed immediately



        public AuroraODEPrim(ISceneChildEntity entity, AuroraODEPhysicsScene parent_scene, bool pisPhysical)

        {

            m_vehicle = new AuroraODEDynamics();

            //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);



            // correct for changed timestep

            PID_D /= (parent_scene.ODE_STEPSIZE*50f); // original ode fps of 50

            PID_G /= (parent_scene.ODE_STEPSIZE*50f);



            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;



            prim_geom = IntPtr.Zero;



            _size = entity.Scale;

            _position = entity.AbsolutePosition;

            fakepos = 0;

            _orientation = entity.GetWorldRotation();

            fakeori = 0;

            _pbs = entity.Shape;

            _parent_entity = entity;



            _parent_scene = parent_scene;

            m_targetSpace = IntPtr.Zero;



            /*

                        m_isphysical = pisPhysical;

                        if (m_isphysical)

                            m_targetSpace = _parent_scene.space;

            */

            m_isphysical = false;



            m_forceacc = Vector3.Zero;

            m_angularforceacc = Vector3.Zero;



            hasOOBoffsetFromMesh = false;

            _triMeshData = IntPtr.Zero;



            CalcPrimBodyData();



            _parent_scene.AddSimulationChange(() => changeadd());

        }



        public ISceneChildEntity ParentEntity

        {

            get { return _parent_entity; }

        }



        public override bool BuildingRepresentation

        {

            get { return m_buildingRepresentation; }

            set

            {

                if (value)

                    m_buildingRepresentation = value;

                //else

                //    _parent_scene.AddSimulationChange(() => m_buildingRepresentation = false);

            }

        }



        public override bool BlockPhysicalReconstruction

        {

            get { return m_blockPhysicalReconstruction; }

            set

            {

                if (value)

                    m_blockPhysicalReconstruction = value;

                else

                    _parent_scene.AddSimulationChange(() =>

                                                          {

                                                              if (value)

                                                                  DestroyBody();

                                                              else

                                                              {

                                                                  m_blockPhysicalReconstruction = false;

                                                                  if (!childPrim)

                                                                      MakeBody();

                                                              }

                                                              if (!childPrim && childrenPrim.Count > 0)

                                                              {

                                                                  foreach (AuroraODEPrim prm in childrenPrim)

                                                                      prm.BlockPhysicalReconstruction = value;

                                                              }

                                                          });

            }

        }



        public PhysicsObject Parent

        {

            get { return _parent; }

        }



        public override int PhysicsActorType

        {

            get { return (int) ActorTypes.Prim; }

        }



        public override uint LocalID

        {

            get { return m_localID; }

            set

            {

                //MainConsole.Instance.Info("[PHYSICS]: Setting TrackerID: " + value);

                m_localID = value;

            }

        }



        public override bool VolumeDetect

        {

            get { return m_isVolumeDetect; }

            set { _parent_scene.AddSimulationChange(() => changevoldtc(value)); }

        }



        public override bool Selected

        {

            set

            {

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

                if ((IsPhysical && !_zeroFlag) || !value)

                    _parent_scene.AddSimulationChange(() => changeSelectedStatus(value));

                else

                    m_isSelected = value;

                if (m_isSelected)

                    disableBodySoft();

            }

        }



        public override bool IsPhysical

        {

            get

            {

                if (childPrim && _parent != null) // root prim defines if is physical or not

                    return ((AuroraODEPrim) _parent).m_isphysical;

                else

                    return m_isphysical;

            }

            set

            {

                _parent_scene.AddSimulationChange(() => changePhysicsStatus(value));

                if (!value) // Zero the remembered last velocity

                    m_lastVelocity = Vector3.Zero;

            }

        }



        public override bool IsTruelyColliding { get; set; }



        public override bool IsColliding

        {

            get { return iscolliding; }

            set

            {

                if (value && _parent != null)

                    _parent.LinkSetIsColliding = value;

                LinkSetIsColliding = value;

                iscolliding = value;

            }

        }



        public override bool LinkSetIsColliding { get; set; }



        public override bool ThrottleUpdates

        {

            get { return m_throttleUpdates; }

            set { m_throttleUpdates = value; }

        }



        public override Vector3 Position

        {

            get

            {

                if (fakepos > 0)

                    return showposition;

                else

                    return _position;

            }

            set

            {

                showposition = value;

                fakepos++;

                _parent_scene.AddSimulationChange(() => changePosition(value));

            }

        }



        public override Vector3 Size

        {

            get { return _size; }

            set

            {

                if (value.IsFinite())

                    _parent_scene.AddSimulationChange(() => changesize(value));

                else

                    MainConsole.Instance.Warn("[PHYSICS]: Got NaN Size on object");

            }

        }





        public override float Mass

        {

            get { return _mass; }

        }



        public override Vector3 Force

        {

            //get { return Vector3.Zero; }

            get { return m_force; }

            set

            {

                if (value.IsFinite())

                {

                    _parent_scene.AddSimulationChange(() => changeforce(value, false));

                }

                else

                {

                    MainConsole.Instance.Warn("[PHYSICS]: NaN in Force Applied to an Object");

                }

            }

        }



        public override int VehicleType

        {

            get { return (int) m_vehicle.Type; }

            set { _parent_scene.AddSimulationChange(() => changeVehicleType(value)); }

        }



        public override Vector3 CenterOfMass

        {

            get

            {

                d.Vector3 dtmp;

                if (IsPhysical && !childPrim && Body != IntPtr.Zero)

                {

                    dtmp = d.BodyGetPosition(Body);

                    return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);

                }

                else if (prim_geom != IntPtr.Zero)

                {

                    d.Quaternion dq;

                    d.GeomCopyQuaternion(prim_geom, out dq);

                    Quaternion q;

                    q.X = dq.X;

                    q.Y = dq.Y;

                    q.Z = dq.Z;

                    q.W = dq.W;



                    Vector3 vtmp = primOOBoffset*q;

                    dtmp = d.GeomGetPosition(prim_geom);

                    return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z);

                }

                else

                    return Vector3.Zero;

            }

        }



        public override PrimitiveBaseShape Shape

        {

            set { _parent_scene.AddSimulationChange(() => changeshape(value)); }

        }



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

                    _parent_scene.AddSimulationChange(() => changevelocity(value));

                else

                {

                    MainConsole.Instance.Warn("[PHYSICS]: Got NaN Velocity in Object");

                }

            }

        }



        public override Vector3 Torque

        {

            get

            {

                if (childPrim || !m_isphysical || Body == IntPtr.Zero)

                    return Vector3.Zero;



                return _torque;

            }



            set

            {

                if (value.IsFinite())

                    _parent_scene.AddSimulationChange(() => changeSetTorque(value));

                else

                    MainConsole.Instance.Warn("[PHYSICS]: Got NaN Torque in Object");

            }

        }



        public override float CollisionScore

        {

            get { return m_collisionscore; }

            set { m_collisionscore = value; }

        }



        public override Quaternion Orientation

        {

            get

            {

                if (fakeori > 0)

                    return showorientation;

                else

                    return _orientation;

            }

            set

            {

                if (QuaternionIsFinite(value))

                {

                    showorientation = value;

                    fakeori++;

                    _parent_scene.AddSimulationChange(() => changeOrientation(value));

                }

                else

                    MainConsole.Instance.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");

            }

        }



        public override Vector3 Acceleration

        {

            get { return _acceleration; }

        }



        public override Vector3 RotationalVelocity

        {

            get

            {

                if (_zeroFlag)

                    return Vector3.Zero;



                if (m_rotationalVelocity.ApproxEquals(Vector3.Zero, 0.00001f))

                    return Vector3.Zero;



                return m_rotationalVelocity;

            }

            set

            {

                if (value.IsFinite())

                    _parent_scene.AddSimulationChange(() => changeangvelocity(value));

                else

                    MainConsole.Instance.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");

            }

        }



        public override float Buoyancy

        {

            get { return m_buoyancy; }

            set { m_buoyancy = value; }

        }



        public override bool FloatOnWater

        {

            set { _parent_scene.AddSimulationChange(() => changefloatonwater(value)); }

        }



        public override void ClearVelocity()

        {

            _velocity = Vector3.Zero;

            _acceleration = Vector3.Zero;

            m_rotationalVelocity = Vector3.Zero;

            m_lastorientation = Orientation;

            m_lastposition = Position;

            m_lastVelocity = _velocity;

            if (Body != null && Body != IntPtr.Zero)

                d.BodySetLinearVel(Body, 0, 0, 0);

        }



        public override void ForceSetVelocity(Vector3 velocity)

        {

            _velocity = velocity;

            m_lastVelocity = velocity;

            if (Body != IntPtr.Zero)

                d.BodySetLinearVel(Body, velocity.X, velocity.Y, velocity.Z);

        }



        public void ForceSetRotVelocity(Vector3 velocity)

        {

            m_rotationalVelocity = velocity;

            if (Body != IntPtr.Zero)

                d.BodySetAngularVel(Body, velocity.X, velocity.Y, velocity.Z);

        }



        public void SetGeom(IntPtr geom)

        {

            prim_geom = geom;

            //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);

            if (prim_geom != IntPtr.Zero)

            {

                d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);

                d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);



                CalcPrimBodyData();



                _parent_scene.actor_name_map[prim_geom] = this;

            }



            if (childPrim)

            {

                if (_parent != null && _parent is AuroraODEPrim)

                {

                    AuroraODEPrim parent = (AuroraODEPrim) _parent;

                    //Console.WriteLine("SetGeom calls ChildSetGeom");

                    parent.ChildSetGeom(this);

                }

            }

            //MainConsole.Instance.Warn("Setting Geom to: " + prim_geom);

        }



        public void enableBodySoft()

        {

            if (!childPrim)

            {

                if (m_isphysical && Body != IntPtr.Zero)

                {

                    d.BodyEnable(Body);

                    if (m_vehicle.Type != Vehicle.TYPE_NONE)

                        m_vehicle.Enable(Body, this, _parent_scene);

                }



                m_disabled = false;

            }

        }



        public void disableBodySoft()

        {

            if (!childPrim)

            {

                m_disabled = true;

                m_vehicle.Disable(this);

                if (IsPhysical && Body != IntPtr.Zero)

                    d.BodyDisable(Body);

            }

        }



        private void MakeBody()

        {

            //            d.Vector3 dvtmp;

            //            d.Vector3 dbtmp;





            if (m_blockPhysicalReconstruction) // building is blocked

                return;



            if (childPrim) // child prims don't get own bodies;

                return;



            if (prim_geom == IntPtr.Zero)

            {

                MainConsole.Instance.Warn("[PHYSICS]: Unable to link the linkset.  Root has no geom yet");

                return;

            }



            if (!m_isphysical) // only physical things get a body

                return;



            if (Body != IntPtr.Zero) // who shouldn't have one already ?

            {

                d.BodyDestroy(Body);

                Body = IntPtr.Zero;

                MainConsole.Instance.Warn("[PHYSICS]: MakeBody called having a body");

            }





            d.Mass objdmass = new d.Mass {};

            d.Matrix3 mymat = new d.Matrix3();

            d.Quaternion myrot = new d.Quaternion();



            Body = d.BodyCreate(_parent_scene.world);



            DMassDup(ref primdMass, out objdmass);



            // rotate inertia

            myrot.X = _orientation.X;

            myrot.Y = _orientation.Y;

            myrot.Z = _orientation.Z;

            myrot.W = _orientation.W;



            d.RfromQ(out mymat, ref myrot);

            d.MassRotate(ref objdmass, ref mymat);



            // set the body rotation and position

            d.BodySetRotation(Body, ref mymat);



            // recompute full object inertia if needed

            if (childrenPrim.Count > 0)

            {

                d.Matrix3 mat = new d.Matrix3();

                d.Quaternion quat = new d.Quaternion();

                d.Mass tmpdmass = new d.Mass {};

                Vector3 rcm;



                rcm.X = _position.X + objdmass.c.X;

                rcm.Y = _position.Y + objdmass.c.Y;

                rcm.Z = _position.Z + objdmass.c.Z;



                lock (childrenPrim)

                {

                    foreach (AuroraODEPrim prm in childrenPrim)

                    {

                        if (prm.prim_geom == IntPtr.Zero)

                        {

                            MainConsole.Instance.Warn(

                                "[PHYSICS]: Unable to link one of the linkset elements, skipping it.  No geom yet");

                            continue;

                        }



                        DMassCopy(ref prm.primdMass, ref tmpdmass);



                        // apply prim current rotation to inertia

                        quat.W = prm._orientation.W;

                        quat.X = prm._orientation.X;

                        quat.Y = prm._orientation.Y;

                        quat.Z = prm._orientation.Z;

                        d.RfromQ(out mat, ref quat);

                        d.MassRotate(ref tmpdmass, ref mat);



                        Vector3 ppos = prm._position;

                        ppos.X += tmpdmass.c.X - rcm.X;

                        ppos.Y += tmpdmass.c.Y - rcm.Y;

                        ppos.Z += tmpdmass.c.Z - rcm.Z;



                        // refer inertia to root prim center of mass position

                        d.MassTranslate(ref tmpdmass,

                                        ppos.X,

                                        ppos.Y,

                                        ppos.Z);



                        d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia



                        // fix prim colision cats



                        d.GeomClearOffset(prm.prim_geom);

                        d.GeomSetBody(prm.prim_geom, Body);

                        prm.Body = Body;

                        d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation

                    }

                }

            }



            d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset

            // associate root geom with body

            d.GeomSetBody(prim_geom, Body);



            d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);

            d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);



            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z);

            // ode wants inertia at center of body

            myrot.W = -myrot.W;

            d.RfromQ(out mymat, ref myrot);

            d.MassRotate(ref objdmass, ref mymat);

            d.BodySetMass(Body, ref objdmass);

            _mass = objdmass.mass;



            m_collisionCategories |= CollisionCategories.Body;

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



            // disconnect from world gravity so we can apply buoyancy

            //            if (!testRealGravity)

            d.BodySetGravityMode(Body, false);



            d.BodySetAutoDisableFlag(Body, true);

            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);

            d.BodySetDamping(Body, .001f, .0002f);

            m_disabled = false;



            d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);

            d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);



            if (m_targetSpace != _parent_scene.space)

            {

                if (d.SpaceQuery(m_targetSpace, prim_geom))

                    d.SpaceRemove(m_targetSpace, prim_geom);



                m_targetSpace = _parent_scene.space;

                d.SpaceAdd(m_targetSpace, prim_geom);

            }



            lock (childrenPrim)

            {

                foreach (AuroraODEPrim prm in childrenPrim)

                {

                    if (prm.prim_geom == IntPtr.Zero)

                        continue;



                    Vector3 ppos = prm._position;

                    d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position



                    prm.m_collisionCategories |= CollisionCategories.Body;

                    prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

                    d.GeomSetCategoryBits(prm.prim_geom, (int) prm.m_collisionCategories);

                    d.GeomSetCollideBits(prm.prim_geom, (int) prm.m_collisionFlags);





                    if (prm.m_targetSpace != _parent_scene.space)

                    {

                        if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))

                            d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);



                        prm.m_targetSpace = _parent_scene.space;

                        d.SpaceAdd(m_targetSpace, prm.prim_geom);

                    }



                    prm.m_disabled = false;

                    _parent_scene.addActivePrim(prm);

                }

            }

            // The body doesn't already have a finite rotation mode set here

            if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)

            {

                createAMotor(m_angularlock);

            }

            if (m_vehicle.Type != Vehicle.TYPE_NONE)

                m_vehicle.Enable(Body, this, _parent_scene);



            _parent_scene.addActivePrim(this);

        }



        private void SetInStaticSpace(AuroraODEPrim prm)

        {

            if (prm.m_targetSpace != null && prm.m_targetSpace == _parent_scene.space)

            {

                if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))

                    d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);

            }

            prm.m_targetSpace = _parent_scene.calculateSpaceForGeom(prm._position);

            d.SpaceAdd(prm.m_targetSpace, prm.prim_geom);

        }





        public void DestroyBody()

            // for now removes all colisions etc from childs, full body reconstruction is needed after this

        {

            //this kills the body so things like 'mesh' can re-create it.

            lock (this)

            {

                if (Body != IntPtr.Zero)

                {

                    _parent_scene.remActivePrim(this);

                    m_collisionCategories &= ~CollisionCategories.Body;

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

                    if (prim_geom != IntPtr.Zero)

                    {

                        d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);

                        d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);

                        UpdateDataFromGeom();

                        SetInStaticSpace(this);

                    }

                    if (!childPrim)

                    {

                        lock (childrenPrim)

                        {

                            foreach (AuroraODEPrim prm in childrenPrim)

                            {

                                _parent_scene.remActivePrim(prm);

                                prm.m_collisionCategories &= ~CollisionCategories.Body;

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

                                if (prm.prim_geom != IntPtr.Zero)

                                {

                                    d.GeomSetCategoryBits(prm.prim_geom, (int) m_collisionCategories);

                                    d.GeomSetCollideBits(prm.prim_geom, (int) m_collisionFlags);

                                    prm.UpdateDataFromGeom();

                                    prm.Body = IntPtr.Zero;

                                    SetInStaticSpace(prm);

                                }

                                prm._mass = prm.primMass;

                            }

                        }

                        m_vehicle.Disable(this);

                        d.BodyDestroy(Body);

                    }

                }

                Body = IntPtr.Zero;

            }

            _mass = primMass;

            m_disabled = true;

        }



        public bool setMesh(AuroraODEPhysicsScene parent_scene, IMesh mesh)

        {

            // This sleeper is there to moderate how long it takes between

            // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object



            //Thread.Sleep(10);



            //Kill Body so that mesh can re-make the geom

            if (IsPhysical && Body != IntPtr.Zero)

            {

                if (childPrim)

                {

                    if (_parent != null)

                    {

                        AuroraODEPrim parent = (AuroraODEPrim) _parent;

                        parent.ChildDelink(this);

                    }

                }

                else

                {

                    DestroyBody();

                }

            }



            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



            if (vertexCount == 0 || indexCount == 0)

            {

                MainConsole.Instance.WarnFormat(

                    "[PHYSICS]: Got invalid mesh on prim at <{0},{1},{2}>. It can be a sculpt with alpha channel in map. Replacing it by a small box.",

                    _position.X, _position.Y, _position.Z);

                _size.X = 0.01f;

                _size.Y = 0.01f;

                _size.Z = 0.01f;

                return false;

            }



            primOOBoffset = mesh.GetCentroid();

            hasOOBoffsetFromMesh = true;



            mesh.releaseSourceMeshData(); // free up the original mesh data to save memory

            if (m_MeshToTriMeshMap.ContainsKey(mesh.Key))

            {

                _triMeshData = m_MeshToTriMeshMap[mesh.Key];

            }

            else

            {

                _triMeshData = d.GeomTriMeshDataCreate();



                d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount,

                                             triStride);

                d.GeomTriMeshDataPreprocess(_triMeshData);

                m_MeshToTriMeshMap[mesh.Key] = _triMeshData;

            }



            try

            {

                if (prim_geom == IntPtr.Zero)

                {

                    SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));

                }

            }

            catch (AccessViolationException)

            {

                MainConsole.Instance.Error("[PHYSICS]: MESH LOCKED");

                return false;

            }



            return true;

        }



        private void changeAngularLock(Vector3 newlock)

        {

            // do we have a Physical object?

            if (Body != IntPtr.Zero)

            {

                //Check that we have a Parent

                //If we have a parent then we're not authorative here

                if (_parent == null)

                {

                    if (!newlock.ApproxEquals(Vector3.One, 0f))

                    {

                        createAMotor(newlock);

                    }

                    else

                    {

                        if (Amotor != IntPtr.Zero)

                        {

                            d.JointDestroy(Amotor);

                            Amotor = IntPtr.Zero;

                        }

                    }

                }

            }

            // Store this for later in case we get turned into a separate body

            m_angularlock = newlock;

        }



        private void changelink(AuroraODEPrim newparent)

        {

            // If the newly set parent is not null

            // create link

            if (_parent == null && newparent != null)

            {

                newparent.ParentPrim(this);

            }

                // If the newly set parent is null

                // destroy link

            else if (_parent != null)

            {

                if (_parent is AuroraODEPrim)

                {

                    if (newparent != _parent)

                    {

                        AuroraODEPrim obj = (AuroraODEPrim) _parent;

                        obj.ChildDelink(this);

                        childPrim = false;



                        if (newparent != null)

                        {

                            newparent.ParentPrim(this);

                        }

                    }

                }

            }



            _parent = newparent;

        }



        // I'm the parent

        // prim is the child

        public void ParentPrim(AuroraODEPrim prim)

        {

            //Console.WriteLine("ParentPrim  " + m_primName);

            if (this.m_localID != prim.m_localID)

            {

                DestroyBody();



                lock (childrenPrim)

                {

#if (!ISWIN)

                    foreach (AuroraODEPrim prm in prim.childrenPrim)

                    {

                        if (!childrenPrim.Contains(prm))

                        {

                            childrenPrim.Add(prm);

                        }

                    }

#else

                    foreach (AuroraODEPrim prm in prim.childrenPrim.Where(prm => !childrenPrim.Contains(prm)))

                    {

                        childrenPrim.Add(prm);

                    }

#endif

                    if (!childrenPrim.Contains(prim)) // must allow full reconstruction

                        childrenPrim.Add(prim);

                }

                //Remove old children

                prim.childrenPrim.Clear();

                prim.childPrim = true;

                prim._parent = this;



                if (prim.Body != IntPtr.Zero)

                {

                    prim.DestroyBody(); // don't loose bodies around

                    prim.Body = IntPtr.Zero;

                }

                MakeBody(); // full nasty reconstruction

            }

        }



        private void ChildSetGeom(AuroraODEPrim odePrim)

        {

            DestroyBody();

            MakeBody();

        }



        private void UpdateChildsfromgeom()

        {

            if (childrenPrim.Count > 0)

            {

                foreach (AuroraODEPrim prm in childrenPrim)

                    prm.UpdateDataFromGeom();

            }

        }



        private void UpdateDataFromGeom()

        {

            if (prim_geom != IntPtr.Zero)

            {

                d.Vector3 lpos = d.GeomGetPosition(prim_geom);

                _position.X = lpos.X;

                _position.Y = lpos.Y;

                _position.Z = lpos.Z;

                d.Quaternion qtmp = new d.Quaternion

                                        {

                                        };

                d.GeomCopyQuaternion(prim_geom, out qtmp);

                _orientation.W = qtmp.W;

                _orientation.X = qtmp.X;

                _orientation.Y = qtmp.Y;

                _orientation.Z = qtmp.Z;

            }

        }



        private void ChildDelink(AuroraODEPrim odePrim)

        {

            // Okay, we have a delinked child.. destroy all body and remake

            if (odePrim != this && !childrenPrim.Contains(odePrim))

                return;



            DestroyBody();



            if (odePrim == this)

            {

                AuroraODEPrim newroot = null;

                lock (childrenPrim)

                {

                    if (childrenPrim.Count > 0)

                    {

                        newroot = childrenPrim[0];

                        childrenPrim.RemoveAt(0);

                        foreach (AuroraODEPrim prm in childrenPrim)

                        {

                            newroot.childrenPrim.Add(prm);

                        }

                        childrenPrim.Clear();

                    }

                    if (newroot != null)

                    {

                        newroot.childPrim = false;

                        newroot._parent = null;

                        newroot.MakeBody();

                    }

                }

            }



            else

            {

                lock (childrenPrim)

                {

                    childrenPrim.Remove(odePrim);

                    odePrim.childPrim = false;

                    odePrim._parent = null;

                    //odePrim.UpdateDataFromGeom ();

                    odePrim.MakeBody();

                }

            }



            MakeBody();

        }



        private void ChildRemove(AuroraODEPrim odePrim)

        {

            // Okay, we have a delinked child.. destroy all body and remake

            if (odePrim != this && !childrenPrim.Contains(odePrim))

                return;



            DestroyBody();



            if (odePrim == this)

            {

                AuroraODEPrim newroot = null;

                lock (childrenPrim)

                {

                    if (childrenPrim.Count > 0)

                    {

                        newroot = childrenPrim[0];

                        childrenPrim.RemoveAt(0);

                        foreach (AuroraODEPrim prm in childrenPrim)

                        {

                            newroot.childrenPrim.Add(prm);

                        }

                        childrenPrim.Clear();

                    }

                    if (newroot != null)

                    {

                        newroot.childPrim = false;

                        newroot._parent = null;

                        newroot.MakeBody();

                    }

                }

                return;

            }

            else

            {

                lock (childrenPrim)

                {

                    childrenPrim.Remove(odePrim);

                    odePrim.childPrim = false;

                    odePrim._parent = null;

                }

            }



            MakeBody();

        }



        private void changeSelectedStatus(bool newsel)

        {

            bool isphys = IsPhysical;



            if (newsel)

            {

                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 (isphys)

                {

                    disableBodySoft();

                }



                if (prim_geom != IntPtr.Zero)

                {

                    d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);

                    d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);

                }



                if (isphys)

                {

                    disableBodySoft();

                }

            }

            else

            {

                m_collisionCategories = CollisionCategories.Geom;



                if (isphys)

                    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)

                {

                    d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);

                    d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);

                }

                if (isphys)

                {

                    if (Body != IntPtr.Zero)

                    {

                        d.BodySetLinearVel(Body, 0f, 0f, 0f);

                        d.BodySetAngularVel(Body, 0f, 0f, 0f);

                        d.BodySetForce(Body, 0, 0, 0);

                        d.BodySetTorque(Body, 0, 0, 0);

                        enableBodySoft();

                    }

                }

            }



            resetCollisionAccounting();

            m_isSelected = newsel;

            if (!m_isSelected)

            {

                _zeroFlag = false;

            }

        }



        //end changeSelectedStatus



        public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)

        {

            hasOOBoffsetFromMesh = false;



            bool havemesh = false;

            //Console.WriteLine("CreateGeom:");

            if (_mesh != null)

            {

                havemesh = setMesh(_parent_scene, _mesh); // this will give a mesh to non trivial known prims

            }



            if (!havemesh)

            {

                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte) Extrusion.Curve1

                    && _size.X == _size.Y && _size.Y == _size.Z)

                {

                    // it's a sphere

                    try

                    {

                        SetGeom(d.CreateSphere(m_targetSpace, _size.X*0.5f));

                    }

                    catch (Exception e)

                    {

                        MainConsole.Instance.Warn("[PHYSICS]: Create sphere failed: {0}", e);

                        return;

                    }

                }

                else

                {

                    try

                    {

                        SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));

                    }

                    catch (Exception e)

                    {

                        MainConsole.Instance.Warn("[PHYSICS]: Create box failed: {0}", e);

                        return;

                    }

                }

            }

        }



        private void RemoveGeom()

        {

            if (prim_geom != IntPtr.Zero)

            {

                _parent_scene.actor_name_map.Remove(prim_geom);

                try

                {

                    d.GeomDestroy(prim_geom);

                    /*

                                        if (_triMeshData != IntPtr.Zero)

                                        {

                                            d.GeomTriMeshDataDestroy(_triMeshData);

                                            _triMeshData = IntPtr.Zero;

                                        }

                     */

                }



                catch (Exception e)

                {

                    MainConsole.Instance.ErrorFormat("[PHYSICS]: PrimGeom destruction failed {1}", e);

                }



                prim_geom = IntPtr.Zero;

            }

            else

            {

                MainConsole.Instance.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD");

            }

            Body = IntPtr.Zero;

            hasOOBoffsetFromMesh = false;

            CalcPrimBodyData();

        }



        public void changeadd()

        {

            // all prims are now created non physical

            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);

            m_targetSpace = targetspace;



            if (_mesh == null)

            {

                if (_parent_scene.needsMeshing(_parent_entity))

                {

                    // Don't need to re-enable body..   it's done in SetMesh

                    _mesh = _parent_scene.mesher.CreateMesh(_parent_entity.Name, _pbs, _size,

                                                            _parent_scene.meshSculptLOD, true);



                    //…