/// <summary>
        /// Creates a part.
        /// </summary>
        /// <param name="primitiveProperties">Properties of the body's shape.</param>
        /// <param name="massProperties">Properties of the body's mass.</param>
        /// <param name="withMainController">Adds a main BasicController for this part.</param>
        public Part(JLG.PrimitiveProperties primitiveProperties, MassProperties massProperties, bool withMainController)
            : base()
        {
            _primitiveProperties = primitiveProperties;
            _massProperties = massProperties;

            if (withMainController)
            {
                // Create Main Controller
                _mainController = new BasicController(this);

                if (PhysicsSystem.CurrentPhysicsSystem != null)
                {
                    PhysicsSystem.CurrentPhysicsSystem.AddController(_mainController);
                }
            }

            _relatedControllers = new Hashtable();
        }
 public override void GetMassProperties(PrimitiveProperties primitiveProperties, out float mass, out Vector3 centerOfMass, out Matrix inertiaTensor)
 {
     mass = 0.0f;
     centerOfMass = Vector3.Zero;
     inertiaTensor = Matrix.Identity;
 }
        public override void GetMassProperties(PrimitiveProperties primitiveProperties, out float mass, out Vector3 centerOfMass, out Matrix inertiaTensor)
        {
            if (primitiveProperties.MassType == PrimitiveProperties.MassTypeEnum.Mass)
            {
                mass = primitiveProperties.MassOrDensity;
            }
            else
            {
                if (primitiveProperties.MassDistribution == PrimitiveProperties.MassDistributionEnum.Solid)
                    mass = GetVolume() * primitiveProperties.MassOrDensity;
                else
                    mass = GetSurfaceArea() * primitiveProperties.MassOrDensity;
            }

            centerOfMass = transform.Position + 0.5f * length * MatrixHelper.GetBackward(transform.Orientation);

            /// todo check solid/shell
            // first cylinder
            float cylinderMass = mass * MathHelper.Pi * radius * radius * length / GetVolume();
            float Ixx = 0.5f * cylinderMass * radius * radius;
            float Iyy = 0.25f * cylinderMass * radius * radius + (1.0f / 12.0f) * cylinderMass * length * length;
            float Izz = Iyy;
            // add ends
            float endMass = mass - cylinderMass;
            Ixx += 0.4f * endMass * radius * radius;
            Iyy += 0.4f * endMass * radius * radius + endMass * (0.5f * length) * (0.5f * length);
            Izz += 0.4f * endMass * radius * radius + endMass * (0.5f * length) * (0.5f * length);

            inertiaTensor = Matrix.Identity;
            inertiaTensor.M11 = Ixx;
            inertiaTensor.M22 = Iyy;
            inertiaTensor.M33 = Izz;

            // transform - e.g. see p664 of Physics-Based Animation
            // todo is the order correct here? Does it matter?

            // Calculate the tensor in a frame at the CoM, but aligned with the world axes
            inertiaTensor = transform.Orientation * inertiaTensor * Matrix.Transpose(transform.Orientation);

            // Transfer of axe theorem
            inertiaTensor.M11 = inertiaTensor.M11 + mass * (centerOfMass.Y * centerOfMass.Y + centerOfMass.Z * centerOfMass.Z);
            inertiaTensor.M22 = inertiaTensor.M22 + mass * (centerOfMass.Z * centerOfMass.Z + centerOfMass.X * centerOfMass.X);
            inertiaTensor.M33 = inertiaTensor.M33 + mass * (centerOfMass.X * centerOfMass.X + centerOfMass.Y * centerOfMass.Y);

            inertiaTensor.M12 = inertiaTensor.M21 = inertiaTensor.M12 - mass * centerOfMass.X * centerOfMass.Y;
            inertiaTensor.M23 = inertiaTensor.M32 = inertiaTensor.M23 - mass * centerOfMass.Y * centerOfMass.Z;
            inertiaTensor.M31 = inertiaTensor.M13 = inertiaTensor.M31 - mass * centerOfMass.Z * centerOfMass.X;
        }
 /// <summary>
 /// Returns the mass, center of mass, and intertia tensor around the origin
 /// </summary>
 /// <param name="primitiveProperties"></param>
 /// <param name="mass"></param>
 /// <param name="centerOfMass"></param>
 /// <param name="inertiaTensor"></param>
 public abstract void GetMassProperties(PrimitiveProperties primitiveProperties,
     out float mass, out Vector3 centerOfMass, out Matrix inertiaTensor);
        /// <summary>
        /// Helper to calculate the combined mass, centre of mass, and
        /// inertia tensor about the origin and the CoM (for the local
        /// primitives) primitiveProperties is an array of properties -
        /// must be the same number as there are primitives
        /// </summary>
        /// <param name="primitiveProperties"></param>
        /// <param name="mass"></param>
        /// <param name="centerOfMass"></param>
        /// <param name="inertiaTensor"></param>
        /// <param name="inertiaTensorCoM"></param>
        public void GetMassProperties(PrimitiveProperties[] primitiveProperties,
            out float mass, out Vector3 centerOfMass, out Matrix inertiaTensor, out Matrix inertiaTensorCoM)
        {
            mass = 0.0f;
            centerOfMass = Vector3.Zero;
            inertiaTensor = Matrix.Identity;
            inertiaTensorCoM = Matrix.Identity;

            for (int prim = primitivesLocal.Count; prim-- != 0; )
            {
                float m;
                Vector3 com;
                Matrix it;
                primitivesLocal[prim].GetMassProperties(primitiveProperties[prim], out m, out com, out it);

                mass += m;
                centerOfMass += m * com;
                inertiaTensor += it;
            }

            if (mass > 0.0f)
            {
                centerOfMass /= mass;

                // Transfer of axe theorem
                inertiaTensorCoM.M11 = inertiaTensor.M11 - mass * (centerOfMass.Y * centerOfMass.Y + centerOfMass.Z * centerOfMass.Z);
                inertiaTensorCoM.M22 = inertiaTensor.M22 - mass * (centerOfMass.Z * centerOfMass.Z + centerOfMass.X * centerOfMass.X);
                inertiaTensorCoM.M33 = inertiaTensor.M33 - mass * (centerOfMass.X * centerOfMass.X + centerOfMass.Y * centerOfMass.Y);

                // CHECK THIS. seems strange for me
                inertiaTensorCoM.M12 = inertiaTensorCoM.M21 = inertiaTensor.M12 + mass * centerOfMass.X * centerOfMass.Y;
                inertiaTensorCoM.M23 = inertiaTensorCoM.M32 = inertiaTensor.M23 + mass * centerOfMass.Y * centerOfMass.Z;
                inertiaTensorCoM.M31 = inertiaTensorCoM.M13 = inertiaTensor.M31 + mass * centerOfMass.Z * centerOfMass.X;

            }
        }
        /// <summary>
        /// Helper to calculate the combined mass, centre of mass, and
        /// inertia tensor about the origin and the CoM (for the local
        /// primitives) primitiveProperties indicates the properties used
        /// for all primitives - so the mass is the total mass
        /// </summary>
        /// <param name="primitiveProperties"></param>
        /// <param name="mass"></param>
        /// <param name="centerOfMass"></param>
        /// <param name="inertiaTensor"></param>
        /// <param name="inertiaTensorCoM"></param>
        public void GetMassProperties(PrimitiveProperties primitiveProperties,
            out float mass, out Vector3 centerOfMass, out Matrix inertiaTensor, out Matrix inertiaTensorCoM)
        {
            mass = 0.0f;
            centerOfMass = Vector3.Zero;
            inertiaTensor = new Matrix();

            float totalWeighting = 0.0f;

            if (primitiveProperties.MassType == PrimitiveProperties.MassTypeEnum.Mass)
            {
                for (int prim = primitivesLocal.Count; prim-- != 0; )
                {
                    if (primitiveProperties.MassDistribution == PrimitiveProperties.MassDistributionEnum.Solid)
                        totalWeighting += primitivesLocal[prim].GetVolume();
                    else
                        totalWeighting += primitivesLocal[prim].GetSurfaceArea();
                }
            }

            for (int prim = primitivesLocal.Count; prim-- != 0; )
            {
                float m; Vector3 com; Matrix it;

                PrimitiveProperties primProperties = primitiveProperties;

                if (primitiveProperties.MassType == PrimitiveProperties.MassTypeEnum.Mass)
                {
                    float weighting = 0.0f;
                    if (primitiveProperties.MassDistribution == PrimitiveProperties.MassDistributionEnum.Solid)
                        weighting = primitivesLocal[prim].GetVolume();
                    else
                        weighting = primitivesLocal[prim].GetSurfaceArea();
                    primProperties.MassOrDensity *= weighting / totalWeighting;
                }

                primitivesLocal[prim].GetMassProperties(primProperties, out m, out com, out it);

                mass += m;
                centerOfMass += m * com;
                inertiaTensor += it;
            }

            inertiaTensorCoM = Matrix.Identity;

            if (mass > 0.0f)
            {
                centerOfMass /= mass;

                // Transfer of axe theorem
                inertiaTensorCoM.M11 = inertiaTensor.M11 - mass * (centerOfMass.Y * centerOfMass.Y + centerOfMass.Z * centerOfMass.Z);
                inertiaTensorCoM.M22 = inertiaTensor.M22 - mass * (centerOfMass.Z * centerOfMass.Z + centerOfMass.X * centerOfMass.X);
                inertiaTensorCoM.M33 = inertiaTensor.M33 - mass * (centerOfMass.X * centerOfMass.X + centerOfMass.Y * centerOfMass.Y);

                // CHECK THIS. seems strange for me
                inertiaTensorCoM.M12 = inertiaTensorCoM.M21 = inertiaTensor.M12 + mass * centerOfMass.X * centerOfMass.Y;
                inertiaTensorCoM.M23 = inertiaTensorCoM.M32 = inertiaTensor.M23 + mass * centerOfMass.Y * centerOfMass.Z;
                inertiaTensorCoM.M31 = inertiaTensorCoM.M13 = inertiaTensor.M31 + mass * centerOfMass.Z * centerOfMass.X;
            }

            if (primitiveProperties.MassType == PrimitiveProperties.MassTypeEnum.Mass)
                mass = primitiveProperties.MassOrDensity;
        }
        /// <summary>
        /// Saves all needed information to recreate an physics world object.
        /// </summary>
        /// <param name="filePath"></param>
        /// <param name="overwrite"></param>
        /// <param name="skin"></param>
        /// <param name="primitiveProperties"></param>
        /// <param name="massProperties">"Null" will cause re-calculation of the mass properties. If you are saving loaded data: Re-calc might cause data loss!</param>
        /// <returns></returns>
        public static bool Save(string filePath, bool overwrite, CollisionSkin skin, PrimitiveProperties primitiveProperties, MassProperties? massProperties)
        {
            if (!File.Exists(filePath) || (File.Exists(filePath) && overwrite))
            {
                PhysicsObjectData data = new PhysicsObjectData();
                data.SetPrimitiveProperties(primitiveProperties);

                for (int i = 0; i < skin.NumPrimitives; i++)
                {
                    data.Add(skin.GetPrimitiveLocal(i), skin.GetMaterialID(i), skin.GetMaterialProperties(i));
                }

                if (massProperties.HasValue)
                {
                    data.MassProperties = massProperties.Value;
                }
                else
                {
                    data.CalculateMassProperties();
                }

                XmlSerializer xmlSerializer = new XmlSerializer(typeof(PhysicsObjectData));
                TextWriter textWriter = new StreamWriter(filePath);

                xmlSerializer.Serialize(textWriter, data);
                textWriter.Close();

                return true;
            }
            else
            {
                MessageBox.Show("Can't write to file \"" + filePath + "\".", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                return false;
            }
        }
        /// <summary>
        /// Loads a physics world object.
        /// </summary>
        /// <param name="filePath"></param>
        /// <param name="skin"></param>
        /// <param name="body"></param>
        /// <param name="massProperties"></param>
        /// <returns></returns>
        public static bool Load(string filePath, out CollisionSkin skin, out Body body, out PrimitiveProperties primitiveProperties, out MassProperties massProperties)
        {
            skin = null;
            body = null;
            primitiveProperties = new PrimitiveProperties(PrimitiveProperties.MassDistributionEnum.Solid, PrimitiveProperties.MassTypeEnum.Mass, 0.001f);
            massProperties = MassProperties.Zero;

            if (File.Exists(filePath))
            {
                XmlSerializer xmlSerializer = new XmlSerializer(typeof(PhysicsObjectData));
                TextReader textReader = new StreamReader(filePath);

                PhysicsObjectData data = (PhysicsObjectData)xmlSerializer.Deserialize(textReader);
                textReader.Close();

                if (data != null && data.MaterialPrimitivePairs != null && data.MaterialPrimitivePairs.Count > 0)
                {
                    body = new JigLibSDX.Physics.Body();
                    skin = new JigLibSDX.Collision.CollisionSkin(body);
                    body.CollisionSkin = skin;

                    primitiveProperties = data.PrimitiveProperties;

                    for (int i = 0; i < data.MaterialPrimitivePairs.Count; i++)
                    {
                        if (data.MaterialPrimitivePairs[i].MaterialID == (int)MaterialTable.MaterialID.UserDefined)
                        {
                            skin.AddPrimitive(data.MaterialPrimitivePairs[i].Primitive, data.MaterialPrimitivePairs[i].MaterialID);
                        }
                        else
                        {
                            skin.AddPrimitive(data.MaterialPrimitivePairs[i].Primitive, data.MaterialPrimitivePairs[i].MaterialProperties);
                        }
                    }

                    massProperties = data.MassProperties;

                    body.BodyInertia = massProperties.InertiaTensorCoM;
                    body.Mass = massProperties.Mass;

                    body.MoveTo(Vector3.Zero, Matrix.Identity);
                    skin.ApplyLocalTransform(new Transform(-massProperties.CenterOfMass, Matrix.Identity));

                    body.EnableBody();
                }

                return true;
            }
            else
            {
                MessageBox.Show("File \"" + filePath + "\" not found.", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                return false;
            }
        }
 public void SetPrimitiveProperties(PrimitiveProperties primitiveProperties)
 {
     _primitiveProperties = primitiveProperties;
 }
        public override void GetMassProperties(PrimitiveProperties primitiveProperties, out float mass, out Vector3 centerOfMass, out Matrix inertiaTensor)
        {
            if (primitiveProperties.MassType == PrimitiveProperties.MassTypeEnum.Mass)
            {
                mass = primitiveProperties.MassOrDensity;
            }
            else
            {
                if (primitiveProperties.MassDistribution == PrimitiveProperties.MassDistributionEnum.Solid)
                    mass = GetVolume() * primitiveProperties.MassOrDensity;
                else
                    mass = GetSurfaceArea() * primitiveProperties.MassOrDensity;
            }

            centerOfMass = GetCentre();

            // First calculate inertia in local frame, then shift to origin
            Vector3 sides = sideLengths;
            // todo check solid/shell
            float Ixx = (1.0f / 12.0f) * mass * (sides.Y * sides.Y + sides.Z * sides.Z);
            float Iyy = (1.0f / 12.0f) * mass * (sides.X * sides.X + sides.Z * sides.Z);
            float Izz = (1.0f / 12.0f) * mass * (sides.X * sides.X + sides.Y * sides.Y);

            inertiaTensor = Matrix.Identity;
            inertiaTensor.M11 = Ixx;
            inertiaTensor.M22 = Iyy;
            inertiaTensor.M33 = Izz;

            // transform - e.g. see p664 of Physics-Based Animation
            // todo is the order correct here? Does it matter?

            // Calculate the tensor in a frame at the CoM, but aligned with the world axes
            inertiaTensor = Matrix.Transpose(transform.Orientation) * inertiaTensor  * transform.Orientation;

            // Transfer of axe theorem
            inertiaTensor.M11 = inertiaTensor.M11 + mass * (centerOfMass.Y * centerOfMass.Y + centerOfMass.Z * centerOfMass.Z);
            inertiaTensor.M22 = inertiaTensor.M22 + mass * (centerOfMass.Z * centerOfMass.Z + centerOfMass.X * centerOfMass.X);
            inertiaTensor.M33 = inertiaTensor.M33 + mass * (centerOfMass.X * centerOfMass.X + centerOfMass.Y * centerOfMass.Y);

            inertiaTensor.M12 = inertiaTensor.M21 = inertiaTensor.M12 - mass * centerOfMass.X * centerOfMass.Y;
            inertiaTensor.M23 = inertiaTensor.M32 = inertiaTensor.M23 - mass * centerOfMass.Y * centerOfMass.Z;
            inertiaTensor.M31 = inertiaTensor.M13 = inertiaTensor.M31 - mass * centerOfMass.Z * centerOfMass.X;
        }
        private void Initialize(Primitive primitive, MaterialProperties materialProperties, PrimitiveProperties primitiveProperties, bool enableBody)
        {
            float mass;
            Vector3 centerOfMass;
            Matrix inertiaTensor;
            Matrix inertiaTensorCoM;

            // Set variables ...
            _primitive = primitive;
            _primitiveProperties = primitiveProperties;
            _materialProperties = materialProperties;

            // Create and link Body and CollisionSkin.
            _body = new Body();
            _skin = new CollisionSkin(_body);
            _body.CollisionSkin = _skin;

            // Add primitive to CollisionSkin.
            _skin.AddPrimitive(primitive, materialProperties);

            // Set body properties.
            _skin.GetMassProperties(primitiveProperties, out mass, out centerOfMass, out inertiaTensor, out inertiaTensorCoM);

            _body.BodyInertia = inertiaTensorCoM;
            _body.Mass = mass;

            // Sync CollisionSkin and Body.
            _body.MoveTo(Vector3.Zero, Matrix.Identity);
            _skin.ApplyLocalTransform(new Transform(-centerOfMass, Matrix.Identity));

            // Enable Body.
            if (enableBody)
            {
                _body.EnableBody();
            }
            else
            {
                _body.DisableBody();
            }
        }
 /// <summary>
 /// Initializes a new BasicObject.
 /// </summary>
 /// <param name="primitive">Primitive that will define the CollisionSkin.</param>
 /// <param name="materialProperties"></param>
 /// <param name="primitiveProperties"></param>
 /// <param name="enableBody"></param>
 public BasicObject(Primitive primitive, MaterialProperties materialProperties, PrimitiveProperties primitiveProperties, bool enableBody)
 {
     Initialize(primitive, materialProperties, primitiveProperties, enableBody);
 }
 /// <summary>
 /// Initializes a new BasicObject. Body is enabled by default.
 /// </summary>
 /// <param name="primitive">Primitive that will define the CollisionSkin.</param>
 /// <param name="materialProperties"></param>
 /// <param name="primitiveProperties"></param>
 public BasicObject(Primitive primitive, MaterialProperties materialProperties, PrimitiveProperties primitiveProperties)
 {
     Initialize(primitive, materialProperties, primitiveProperties, true);
 }