using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VisiMath;
using VisiMath.Geometry;
using VisimBox.Scene;
using System.ComponentModel;

namespace VisimBox.Physics
{
    /// <summary>
    /// Type of used Damping
    /// </summary>
    public enum DampingType
    {
        /// <summary>
        /// None speed damping
        /// </summary>
        None,
        /// <summary>
        /// Linear (frame rate dependent, fast)
        /// </summary>
        Linear,
        /// <summary>
        /// Exponential timed (frame rate independent)
        /// </summary>
        Timed
    };

    /// <summary>
    /// Base physical node
    /// </summary>
    public abstract class PhysNode : RenderableNode
    {
        /// <summary>
        /// physical world in which the physical node is operating
        /// </summary>
        public PhysicalScene PhysWorld
        {
            get; //note - set by registerbody or other register type function
            set;
        }


        public bool AutoRegister
        {
            //notimplemented
            get;
            set;
        }

        //not implemented base physnode

        public override RenderFlags DefaultRenderFlags
        {
            get
            {
                return Scene.RenderFlags.PhysicalObject;
            }
        }

        public PhysNode()
        {
        }

        /// <summary>
        /// XML reader constructor
        /// </summary>
        /// <param name="reader">XML reader</param>
        public PhysNode(System.Xml.XmlReader reader)
            : base(reader)
        {
        }

        protected override void CopyData(Node targetNode)
        {
            base.CopyData(targetNode);

            PhysNode mynode = targetNode as PhysNode;
            if (mynode == null)
            {
                VisimBox.Console.DebugLine(FullName + " asked to do copy to different node type!");
                return;
            }
            if (PhysWorld != null)
                mynode.PhysWorld = PhysWorld;
        }


    }

    /// <summary>
    /// Object describing rigid body
    /// </summary>
    public class RigidBody : PhysNode, IContactGenerator
    {
        /// <summary>
        /// world node used for calculations
        /// </summary>
        protected Node WorldNode;

        #region standard values

        protected Vector3 _velocity, _acceleration,  _force, _torque;
        
        protected bool _sleeping, _isstatic, _cansleep = true;
        protected bool _standardGravity = true;

        [Description("Physical position of the node")]
        [Category("Position and motion data")]
        [TypeConverter(typeof(VectorConvertor))]
        public Vector3 Position { get { return Trans.Position; } set { Trans.Position = value; } }

        /// <summary>
        /// Linear velocity
        /// </summary>
        [Description("Linear velocity of the node")]
        [Category("Position and motion data")]
        [TypeConverter(typeof(VectorConvertor))]
        public Vector3 Velocity { get { return _velocity; } set { _velocity = value; } }

        /// <summary>
        /// Increases velocity by speficied ammount;
        /// </summary>
        /// <param name="vel">velocity change</param>
        public void AddVelocity(Vector3 vel)
        {
            AddVelocity(ref vel);
        }

        /// <summary>
        /// Increases velocity by speficied ammount;
        /// </summary>
        /// <param name="vel">velocity change</param>
        public void AddVelocity(ref Vector3 vel)
        {
            _velocity.Add(ref vel);
        }

        /// <summary>
        /// Linear acceleration
        /// </summary>
        [Description("Linear acceleration of the node")]
        [Category("Position and motion data")]
        [TypeConverter(typeof(VectorConvertor))]
        public Vector3 Acceleration { get { return _acceleration; } set { _acceleration = value; } }

        protected Vector3 _accelLastFrame;
        
        /// <summary>
        /// Acceleration in the last frame
        /// </summary>
        [Description("Final acceleration in the last frame")]
        [Category("Position and motion data")]
        [TypeConverter(typeof(VectorConvertor))]
        public Vector3 AccelerationLastFrame { get { return _accelLastFrame; } }

        protected Vector3 _angularVelocity, _angularAcceleration;

        [Description("Physical rotation of the node")]
        [Category("Position and motion data")]
        public Quaternion Rotation
        {
            get { return Trans.Rotation; }
            set { Trans.Rotation = value; }
        }

        /// <summary>
        /// Angular velocity
        /// </summary>
        [Description("Angular velocity of the node")]
        [Category("Position and motion data")]
        [TypeConverter(typeof(VectorConvertor))]
        public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } }

        /// <summary>
        /// Increases angular velocity by specified ammount
        /// </summary>
        /// <param name="vec">Angular velocity change</param>
        public void AddAngularVelocity(Vector3 vec)
        {
            AddAngularVelocity(ref vec);
        }

        /// <summary>
        /// Increases angular velocity by specified ammount
        /// </summary>
        /// <param name="vec">Angular velocity change</param>
        public void AddAngularVelocity(ref Vector3 vec)
        {
            _angularVelocity.Add(ref vec);
        }

        /// <summary>
        /// Angular acceleration
        /// </summary>
        [Description("Angular acceleration of the node")]
        [Category("Position and motion data")]
        [TypeConverter(typeof(VectorConvertor))]
        public Vector3 AngularAcceleration { get { return _angularAcceleration; } set { _angularAcceleration = value; } }


        /// <summary>
        /// Speed damping
        /// </summary>
        [Description("Linear damping (forced slowdown of linear velocity - 1 means no, 0 absolute slowdown)")]
        [Category("Physical settings")]
        public float Damping { get; set; }

        /// <summary>
        /// Type of the linear speed damping
        /// </summary>
        [Description("Linear damping type")]
        [Category("Physical settings")]
        public DampingType DampingType { get; set; }

        /// <summary>
        /// Speed damping
        /// </summary>
        [Description("Angular damping (forced slowdown of angular velocity - 1 means no, 0 absolute slowdown)")]
        [Category("Physical settings")]
        public float AngularDamping { get; set; }

        /// <summary>
        /// Type of the angular speed damping
        /// </summary>
        [Description("Angular damping type")]
        [Category("Physical settings")]
        public DampingType AngularDampingType { get; set; }


        /// <summary>
        /// Density of the body
        /// </summary>
        [Description("Density (requires correct volumetric geometric object to work)")]
        [Category("Physical settings")]
        public float Density
        {
            get
            {
                if (PhysicalBody == null)
                    return 0;
                return PhysicalBody.Volume * _invertedMass;
            }
            set
            {
                if (PhysicalBody == null)
                    return;
                _invertedMass = value/PhysicalBody.Volume;
            }
        }

        /// <summary>
        /// Total weight of the body
        /// </summary>
        [Description("Total mass (weight) of the body")]
        [Category("Physical settings")]
        public float Mass { get { return 1 / InvertedMass; } set { InvertedMass = 1 / value; } }

        /// <summary>
        /// Inverted total weigth of the body
        /// </summary>
        [Description("Inverted mass of the body (0 for static/kinematic type onmovable by other forces)")]
        [Category("Physical settings")]
        public float InvertedMass
        { 
            get
            {
                return _invertedMass;
            } 
            set
            {
                if (_invertedMass != value)
                {
                    _invertedMass = value;
                    _validData = false;
                }
            } 
        }

        protected float _invertedMass;
        protected Matrix3 _invertedInertia = Matrix3.Identitity;
        protected bool _validData = false;

        /// <summary>
        /// Inertia tensor for rotations (calculates invert from stored _invertedInertia)
        /// </summary>
        [Description("Inertia tensor for the object")]
        [Category("Physical settings")]
        public Matrix3 InertiaTensor
        {
            get
            {
                try
                {
                    return Matrix3.Invert(ref _invertedInertia);
                }
                catch
                {
                    return Matrix3.Identitity;
                }
            }

            set
            {
                //todo!!! add check if ok
                _validData = false;
                _invertedInertia = Matrix3.Invert(ref value);
            }
        }

        /// <summary>
        /// Inverted inertia tensor for rotations
        /// </summary>
        [Description("Inverted inertia tensor for the object")]
        [Category("Physical settings")]
        public Matrix3 InvertedInertia
        {
            get
            {
                return _invertedInertia;
            }

            set
            {
                _validData = false;
                _invertedInertia = value;
            }
        }

        /// <summary>
        /// Inverted inertia converted to world coordinates
        /// </summary>
        protected Matrix3 InvertedInertiaWorld = new Matrix3();


        /// <summary>
        /// Is object sleeping (ie no updates unless hit from outside
        /// </summary>
        [Description("Is object sleeping (inactive until woke up by collision or user callback")]
        [Category("Position and motion data")]
        public bool Sleeping
        { 
            get { return _sleeping; } 
            set 
            { 
                _sleeping = value;
                if (_sleeping)
                {
                    _velocity.Assign(0);
                    _angularVelocity.Assign(0);
                }
                else
                    Motion = 10 * MotionEpsilon;
            } 
        }

        /// <summary>
        /// Motion of the object (used to set sleep state)
        /// </summary>
        [Description("Total motion value, object will sleep when it falls below MotionEpsilon")]
        [Category("Position and motion data")]
        public float Motion;

        /// <summary>
        /// Motion epsilon (if motion is below this value, object will sleep
        /// </summary>
        [Description("Motion epsilon, object will sleep when it's motion is below this value")]
        [Category("Position and motion data")]
        public float MotionEpsilon { get; set; }

        /// <summary>
        /// Can object sleep?
        /// </summary>
        [Description("Can object sleep when his motion gets too low? (set false for always active objects)")]
        [Category("Position and motion data")]
        public bool CanSleep
        {
            get
            {
                return _cansleep;
            }
            set
            {
                _cansleep = value;
                if (!_cansleep && _sleeping)
                    Sleeping = false;
            }
        }

        /// <summary>
        /// Is object static immovable object?
        /// </summary>
        [Description("Is object static immovable object? will be always true for objects with inifinite mass.")]
        [Category("Physical settings")]
        public bool IsStaticObject
        { 
            get 
            {
                return _isstatic || (InvertedMass == 0);
            }
            set
            {
                _isstatic = value;
            }
        }

        /// <summary>
        /// Does global world gravity apply to this object?
        /// </summary>
        [Description("Does global world gravity apply to this object?")]
        [Category("Physical settings")]
        public bool HasStandardGravity
        {
            get
            {
                return _standardGravity;
            }

            set
            {
                if (_standardGravity != value)
                {
                    if (PhysWorld != null)
                    {
                        if (value)
                        {
                            //start using gravity
                            this._acceleration.Add(PhysWorld.Gravity);
                        }
                        else
                        {
                            //disable gravity
                            this._acceleration.Substract(PhysWorld.Gravity);
                        }
                    }
                    else
                    {
                        Console.DebugLine("Attempt to change gravity for unregistered physical body!");
                    }
                }
            }
        }

        #endregion

        /// <summary>
        /// Material describing the surface of the body
        /// </summary>
        public PhysMaterial Material { get; set; }  //todo check for null material

        //public IVolumetric PhysicalBody;
        public IVolumetric PhysicalBody { get; set; } //todo, check for null

        public override RenderFlags DefaultRenderFlags
        {
            get
            {
                return Scene.RenderFlags.PhysicalObject;
            }
        }

        /// <summary>
        /// Do NOT use unless you're really sure!
        /// </summary>
        public RigidBody() : this(null as Node)
        {
        }

        public RigidBody(System.Xml.XmlReader reader) : base(reader)
        {
            //registering physworld must be done by deserializer
        }


        public RigidBody(Node physworld)
        {
            WorldNode = physworld;
            Damping = 0.995f;
            AngularDamping = 0.995f;
            DampingType = Physics.DampingType.Timed;
            AngularDampingType = Physics.DampingType.Timed;
            Motion = 100;
            MotionEpsilon = 0.2f;
            Trans = new QTransformation();
            //Visible = false;
        }

        /// <summary>
        /// Integrates states of the object
        /// </summary>
        /// <param name="time"></param>
        /// <returns></returns>
        public virtual bool Intergrator(float time)
        {
            //time should be > 0!

            _accelLastFrame = _acceleration;
            _accelLastFrame.AddScaled(_force, _invertedMass);

            Vector3 angularacc = InvertedInertiaWorld.Multiply(ref _torque);

            _velocity.AddScaled(ref _accelLastFrame, time); //change velocity on acceleration and impulse

            _angularVelocity.AddScaled(ref angularacc, time); //change rotation on acceleration and impulse
            
            switch (DampingType)
            {
                case Physics.DampingType.Linear:
                    _velocity.Mul(Damping);                    
                    break;
                case Physics.DampingType.Timed:
                    _velocity.Mul(MathTools.Pow(Damping, time));
                    break;
            }

            switch (AngularDampingType)
            {
                case Physics.DampingType.Linear:
                    _angularVelocity.Mul(AngularDamping);
                    break;
                case Physics.DampingType.Timed:
                    _angularVelocity.Mul(MathTools.Pow(AngularDamping, time));
                    break;
            }

            Vector3 positionchange = new Vector3(_velocity, time);
            positionchange.AddScaled(ref _accelLastFrame, 0.5f * time * time);
            Trans.Move(ref positionchange);

            Quaternion q = Trans.Rotation;
            q.AddVector(ref _angularVelocity, time);
            //add rotation from acceleration too?
            q.Normalize();
            Trans.Rotation = q;


            if (_cansleep)
            {
                float currentmotion = _velocity.LengthSquared + _angularVelocity.LengthSquared;

                float bias = MathTools.Pow(0.5f, time);
                Motion = bias * Motion + (1 - bias) * currentmotion;


                if (Motion < MotionEpsilon)
                {
                    //object is not moving, no velocity... - set sleeping
                    Sleeping = true;
                    return false;
                }
                else if (Motion > 10 * MotionEpsilon)
                    Motion = 10 * MotionEpsilon;
            }
            return true;
        }

        /// <summary>
        /// Clears force accumulator
        /// </summary>
        public void ClearAccumulator()
        {
            _force.Assign(0);
            _torque.Assign(0);
        }

        /// <summary>
        /// Performs physical time tick
        /// </summary>
        /// <param name="time">Elapsed time</param>
        /// <returns>True if object changed</returns>
        public virtual bool PhysicalTimeTick(double time)
        {
            if (_sleeping)
            {
                return false;
            }
            bool rval = Intergrator((float)time);
            ClearAccumulator();
            return rval;
        }


        /// <summary>
        /// Gives object force impulse (working directly on the centre of the mass of the object)
        /// </summary>
        /// <param name="force">Force vector</param>
        public void AddForce(Vector3 force)
        {
            AddForce(ref force);
        }

        /// <summary>
        /// Gives object force impulse (working directly on the centre of the mass of the object)
        /// </summary>
        /// <param name="force">Force vector</param>
        public virtual void AddForce(ref Vector3 force)
        {
            _force.Add(ref force);
            _sleeping = false;
        }

        /// <summary>
        /// Hit object on the specified point (in local coordinates) with specified force
        /// </summary>
        /// <param name="location">Position on the body in local coordinates</param>
        /// <param name="force">Force</param>
        public void AddForce(Vector3 location, Vector3 force)
        {
            AddForce(ref location, ref force);
        }

        /// <summary>
        /// Hit object on the specified point (in local coordinates) with specified force
        /// </summary>
        /// <param name="location">Position on the body in local coordinates</param>
        /// <param name="force">Force</param>
        public void AddForce(ref Vector3 location, ref Vector3 force)
        {
            Vector3 vec = location;
            vec.Substract(ref location);

            _force.Add(ref force);

            Vector3 torq = vec.Cross(ref force);

            _torque.Add(ref torq);

            _sleeping = false;
        }

        /// <summary>
        /// Hit object on the specified point (in world coordinates) with specified force
        /// </summary>
        /// <param name="location">Position on the body in WORLD coordinates</param>
        /// <param name="force">Force</param>
        public void AddForceWorld(Vector3 location, Vector3 force)        
        {
            AddForceWorld(ref location, ref force);
        }

        /// <summary>
        /// Hit object on the specified point (in world coordinates) with specified force
        /// </summary>
        /// <param name="location">Position on the body in WORLD coordinates</param>
        /// <param name="force">Force</param>
        public void AddForceWorld(ref Vector3 location, ref Vector3 force)
        {
            Vector3 worldpoint = GetPointInWorldSpace(location);
            AddForce(ref worldpoint, ref force);
        }

        /// <summary>
        /// Adds torque to object
        /// </summary>
        /// <param name="torq">torque</param>
        public void AddTorque(Vector3 torq)
        {
            AddTorque(ref torq);
        }

        /// <summary>
        /// Adds torque to object
        /// </summary>
        /// <param name="torq">torque</param>
        public void AddTorque(ref Vector3 torq)
        {
            _torque.Add(ref torq);
        }


        /// <summary>
        /// Updates derived values
        /// </summary>
        public void UpdateValues()
        {
            Quaternion q = Trans.Rotation;
            q.Normalize();
            Trans.Rotation = q;

            //inverted inertia matrix
            //automaticaly optimized code (taken from cyclone)

            Matrix4 mat = Trans.TransMatrix;

            float t4 = mat.m00 * InvertedInertia.m00 + mat.m01 * InvertedInertia.m10 + mat.m02 * InvertedInertia.m20;
            float t9 = mat.m00 * InvertedInertia.m01 + mat.m01 * InvertedInertia.m11 + mat.m02 * InvertedInertia.m21;
            float t14 = mat.m00 * InvertedInertia.m02 + mat.m01 * InvertedInertia.m12 + mat.m02 * InvertedInertia.m22;
            float t28 = mat.m10 * InvertedInertia.m00 + mat.m11 * InvertedInertia.m10 + mat.m12 * InvertedInertia.m20;
            float t33 = mat.m10 * InvertedInertia.m01 + mat.m11 * InvertedInertia.m11 + mat.m12 * InvertedInertia.m21;
            float t38 = mat.m10 * InvertedInertia.m02 + mat.m11 * InvertedInertia.m12 + mat.m12 * InvertedInertia.m22;
            float t52 = mat.m20 * InvertedInertia.m00 + mat.m21 * InvertedInertia.m10 + mat.m22 * InvertedInertia.m20;
            float t57 = mat.m20 * InvertedInertia.m01 + mat.m21 * InvertedInertia.m11 + mat.m22 * InvertedInertia.m21;
            float t62 = mat.m20 * InvertedInertia.m02 + mat.m21 * InvertedInertia.m12 + mat.m22 * InvertedInertia.m22;


            InvertedInertiaWorld.m00 = t4 * mat.m00 + t9 * mat.m01 + t14 * mat.m02;
            InvertedInertiaWorld.m01 = t4 * mat.m10 + t9 * mat.m11 + t14 * mat.m12;
            InvertedInertiaWorld.m02 = t4 * mat.m20 + t9 * mat.m21 + t14 * mat.m22;
            InvertedInertiaWorld.m10 = t28 * mat.m00 + t33 * mat.m01 + t38 * mat.m02;
            InvertedInertiaWorld.m11 = t28 * mat.m10 + t33 * mat.m11 + t38 * mat.m12;
            InvertedInertiaWorld.m12 = t28 * mat.m20 + t33 * mat.m21 + t38 * mat.m22;
            InvertedInertiaWorld.m20 = t52 * mat.m00 + t57 * mat.m01 + t62 * mat.m02;
            InvertedInertiaWorld.m21 = t52 * mat.m10 + t57 * mat.m11 + t62 * mat.m12;
            InvertedInertiaWorld.m22 = t52 * mat.m20 + t57 * mat.m21 + t62 * mat.m22;
        }

        /// <summary>
        /// Returns location of point in local space to the world space
        /// </summary>
        /// <param name="point">position in local space</param>
        /// <returns>position in world space</returns>
        public Vector3 GetPointInWorldSpace(Vector3 point)
        {
            return Trans.GetPointInWorldSpace(point);
        }

        /// <summary>
        /// Returns location of point in world space to the local space
        /// </summary>
        /// <param name="point">position in world space</param>
        /// <returns>position in local space</returns>
        public Vector3 GetPointInLocalSpace(Vector3 point)
        {
            Trans.TransMatrix.TransformInverted(ref point);
            return point;
        }



        /// <summary>
        /// Physical Scene callback - node should check for any collisions and save them at physical contact at index and beyond
        /// Note: node should NOT increase the contact list size (HW limit)
        /// </summary>
        /// <param name="List">List to save contact into</param>
        /// <param name="index">Starting index</param>
        /// <returns>Number of added contacts</returns>
        public virtual int AddContact(List<PhysicalContact> List, int index)
        {
            return 0;
        }


        public override bool ReadInternalData(System.Xml.XmlReader reader)
        {
            if (base.ReadInternalData(reader))
                return true;
            switch (reader.Name)
            {
                case "Sleeping":
                    Sleeping = XMLHelper.ReadAsBool(reader);
                    break;
                case "Static":
                    Sleeping = XMLHelper.ReadAsBool(reader);
                    break;
                case "CanSleep":
                    Sleeping = XMLHelper.ReadAsBool(reader);
                    break;
                case "Material":
                    Material = new PhysMaterial(reader);
                    break;
                case "Body":
                    IGeometry geobody = GeoFactory.ReadXml(reader);
                    PhysicalBody = geobody as IVolumetric;
                    if (PhysicalBody == null)
                        Console.DebugLine("RigidBody XML loading - rigid body loading failed!");
                    break;
                case "UseWorldGravity":
                    HasStandardGravity = XMLHelper.ReadAsBool(reader);
                    break;

                case "Mass":
                    Mass = XMLHelper.ReadAsFloat(reader);
                    break;
                case "Velocity":
                    _velocity.ReadXml(reader);
                    break;
                case "Acceleration":
                    _acceleration.ReadXml(reader);
                    break;
                case "Force":
                    _force.ReadXml(reader);
                    break;
                case "Torque":
                    _torque.ReadXml(reader);
                    break;
                case "AccelLastFrame":
                    _accelLastFrame.ReadXml(reader);
                    break;
                case "AngularVelocity":
                    _angularVelocity.ReadXml(reader);
                    break;
                case "AngularAcceleration":
                    _angularAcceleration.ReadXml(reader);
                    break;
                case "Motion":
                    Motion = XMLHelper.ReadAsFloat(reader);
                    break;
                case "MotionEpsilon":
                    MotionEpsilon = XMLHelper.ReadAsFloat(reader);
                    break;
                case "Damping":
                    Damping = XMLHelper.FloatFromString(reader["Value"]);
                    switch (reader["Type"])
                    {
                        case "Linear":
                            DampingType = Physics.DampingType.Linear;
                            break;
                        case "Timed":
                            DampingType = Physics.DampingType.Timed;
                            break;
                        default:
                            DampingType = Physics.DampingType.None;
                            break;
                    }
                    break;
                case "AngularDamping":
                    AngularDamping = XMLHelper.FloatFromString(reader["Value"]);
                    switch (reader["Type"])
                    {
                        case "Linear":
                            AngularDampingType = Physics.DampingType.Linear;
                            break;
                        case "Timed":
                            AngularDampingType = Physics.DampingType.Timed;
                            break;
                        default:
                            AngularDampingType = Physics.DampingType.None;
                            break;
                    }
                    break;

                default:
                    return false;

            }
            return true;
        }

        protected override void CopyData(Node targetNode)
        {
            base.CopyData(targetNode);

            RigidBody mynode = targetNode as RigidBody;
            if (mynode == null)
            {
                VisimBox.Console.DebugLine(FullName + " asked to do copy to different node type!");
                return;
            }

            if (Material != null)
                mynode.Material = new PhysMaterial(Material);
            if (PhysicalBody != null)
                mynode.PhysicalBody = PhysicalBody;
            mynode._invertedMass = this._invertedMass;
            mynode._invertedInertia = this._invertedInertia;
            mynode._standardGravity = this._standardGravity;
            mynode._acceleration = this._acceleration;
            mynode._accelLastFrame = this._accelLastFrame;
            mynode._angularAcceleration = this._angularAcceleration;
            mynode._angularVelocity = this._angularVelocity;
            mynode._cansleep = this._cansleep;
            mynode._force = this._force;
            mynode._isstatic = this._isstatic;
            mynode._sleeping = this._sleeping;
            mynode._validData = false;
            mynode._velocity = this._velocity;
            mynode.Motion = this.Motion;
            mynode.MotionEpsilon = this.MotionEpsilon;
            mynode.Damping = this.Damping;
            mynode.DampingType = this.DampingType;
            mynode.AngularDamping = this.AngularDamping;
            mynode.AngularDampingType = this.AngularDampingType;
        }


        public override void WriteInternalData(System.Xml.XmlWriter writer)
        {
            base.WriteInternalData(writer);

            writer.WriteStartElement("Body");
            PhysicalBody.WriteXml(writer);
            writer.WriteEndElement();

            if (Material != null)
            {
                Material.WriteXml(writer, "Material");
            }

            writer.WriteElementString("Mass", Mass.ToString(XMLHelper.CF));
            //inverted inertia calculated from physicalbody

            writer.WriteElementString("UseWorldGravity", HasStandardGravity.ToString());


            _velocity.WriteXml(writer, "Velocity");
            _acceleration.WriteXml(writer, "Acceleration");
            _force.WriteXml(writer, "Force");
            _torque.WriteXml(writer, "Torque");
            _accelLastFrame.WriteXml(writer, "AccelLastFrame");
            _angularVelocity.WriteXml(writer, "AngularVelocity"); 
            _angularAcceleration.WriteXml(writer, "AngularAcceleration");

            writer.WriteElementString("Motion", Motion.ToString(XMLHelper.CF));
            writer.WriteElementString("MotionEpsilon", MotionEpsilon.ToString(XMLHelper.CF));

            writer.WriteElementString("Sleeping", _sleeping.ToString(XMLHelper.CF));
            writer.WriteElementString("Static", _isstatic.ToString(XMLHelper.CF));
            writer.WriteElementString("CanSleep", _cansleep.ToString(XMLHelper.CF));

            writer.WriteStartElement("Damping");
            writer.WriteAttributeString("Type", DampingType.ToString());
            writer.WriteAttributeString("Value", Damping.ToString(XMLHelper.CF));
            writer.WriteEndElement();
            writer.WriteStartElement("AngularDamping");
            writer.WriteAttributeString("Type", AngularDampingType.ToString());
            writer.WriteAttributeString("Value", AngularDamping.ToString(XMLHelper.CF));
            writer.WriteEndElement();
        }


        /// Activate object in the physical scene. If staticobject is set to true, object will be static inmovable (for terrain)
        //public virtual void Activate(bool staticobject = false);


        public override void RenderGeometry(Renderer.RendererEventArgs args)
        {
            return;
            //throw new NotImplementedException();
        }
    }
}
