xnabox2demo/Box2D.XNA/Dynamics/Body.cs

/*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
* 
* This software is provided 'as-is', without any express or implied 
* warranty.  In no event will the authors be held liable for any damages 
* arising from the use of this software. 
* Permission is granted to anyone to use this software for any purpose, 
* including commercial applications, and to alter it and redistribute it 
* freely, subject to the following restrictions: 
* 1. The origin of this software must not be misrepresented; you must not 
* claim that you wrote the original software. If you use this software 
* in a product, an acknowledgment in the product documentation would be 
* appreciated but is not required. 
* 2. Altered source versions must be plainly marked as such, and must not be 
* misrepresented as being the original software. 
* 3. This notice may not be removed or altered from any source distribution. 
*/

using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    /// The body type.
    /// static: zero mass, zero velocity, may be manually moved
    /// kinematic: zero mass, non-zero velocity set by user, moved by solver
    /// dynamic: positive mass, non-zero velocity determined by forces, moved by solver
    public enum BodyType
    {
        Static,
        Kinematic,
        Dynamic,
    }

    /// A body definition holds all the data needed to construct a rigid body.
    /// You can safely re-use body definitions. Shapes are added to a body after construction.
    public class BodyDef
    {
        /// This constructor sets the body definition default values.
        public BodyDef()
        {
            userData = null;
            position = new Vector2(0.0f, 0.0f);
            angle = 0.0f;
            linearVelocity = new Vector2(0.0f, 0.0f);
            angularVelocity = 0.0f;
            linearDamping = 0.0f;
            angularDamping = 0.0f;
            allowSleep = true;
            awake = true;
            fixedRotation = false;
            bullet = false;
            type = BodyType.Static;
		    active = true;
            inertiaScale = 1.0f;
        }

        /// The body type: static, kinematic, or dynamic.
        /// Note: if a dynamic body would have zero mass, the mass is set to one.
        public BodyType type;

        /// The world position of the body. Avoid creating bodies at the origin
        /// since this can lead to many overlapping shapes.
        public Vector2 position;

        /// The world angle of the body in radians.
        public float angle;

        /// The linear velocity of the body's origin in world co-ordinates.
        public Vector2 linearVelocity;

        /// The angular velocity of the body.
        public float angularVelocity;

        /// Linear damping is use to reduce the linear velocity. The damping parameter
        /// can be larger than 1.0f but the damping effect becomes sensitive to the
        /// time step when the damping parameter is large.
        public float linearDamping;

        /// Angular damping is use to reduce the angular velocity. The damping parameter
        /// can be larger than 1.0f but the damping effect becomes sensitive to the
        /// time step when the damping parameter is large.
        public float angularDamping;

        /// Set this flag to false if this body should never fall asleep. Note that
        /// this increases CPU usage.
        public bool allowSleep;

        /// Is this body awake or sleeping?
        public bool awake;

        /// Should this body be prevented from rotating? Useful for characters.
        public bool fixedRotation;

        /// Is this a fast moving body that should be prevented from tunneling through
        /// other moving bodies? Note that all bodies are prevented from tunneling through
        /// kinematic and static bodies. This setting is only considered on dynamic bodies.
        /// @warning You should use this flag sparingly since it increases processing time.
        public bool bullet;

        /// Does this body start out active?
	    public bool active;

        /// Use this to store application specific body data.
        public object userData;

        /// Experimental: scales the inertia tensor.
        public float inertiaScale;
    };


    [Flags]
    public enum BodyFlags
    {
        None            = 0,
        Island          = (1 << 0),
        Awake           = (1 << 1),
        AutoSleep       = (1 << 2),
        Bullet          = (1 << 3),
        FixedRotation   = (1 << 4),
        Active          = (1 << 5),
        Toi             = (1 << 6),
    }

    public class Body
    {

        /// Set the type of this body. This may alter the mass and velocity.
	    public void SetType(BodyType type)
        {
            if (_type == type)
            {
                return;
            }

            _type = type;

            ResetMassData();

            if (_type == BodyType.Static)
            {
                _linearVelocity = Vector2.Zero;
                _angularVelocity = 0.0f;
            }

            SetAwake(true);

            _force = Vector2.Zero;
            _torque = 0.0f;

            // Since the body type changed, we need to flag contacts for filtering.
            for (ContactEdge ce = _contactList; ce != null; ce = ce.Next)
            {
                ce.Contact.FlagForFiltering();
            }
        }

	    /// Get the type of this body.
        public new BodyType GetType()
        {
            return _type;
        }

	    /// Creates a fixture and attach it to this body. Use this function if you need
	    /// to set some fixture parameters, like friction. Otherwise you can create the
	    /// fixture directly from a shape.
	    /// If the density is non-zero, this function automatically updates the mass of the body.
	    /// Contacts are not created until the next time step.
	    /// @param def the fixture definition.
	    /// @warning This function is locked during callbacks.
	    public Fixture CreateFixture(FixtureDef def)
        {
            Debug.Assert(_world.IsLocked == false);
	        if (_world.IsLocked == true)
	        {
		        return null;
	        }

            Fixture fixture = new Fixture();
            fixture.Create(this, def);

            if ((_flags & BodyFlags.Active) == BodyFlags.Active)
            {
                BroadPhase broadPhase = _world._contactManager._broadPhase;
                fixture.CreateProxies(broadPhase, ref _xf);
            }

	        fixture._next = _fixtureList;
	        _fixtureList = fixture;
	        ++_fixtureCount;

	        fixture._body = this;


            // Adjust mass properties if needed.
            if (fixture._density > 0.0f)
            {
                ResetMassData();
            }

            // Let the world know we have a new fixture. This will cause new contacts
            // to be created at the beginning of the next time step.
	        _world._flags |= WorldFlags.NewFixture;

	        return fixture;
        }

	    /// Creates a fixture from a shape and attach it to this body.
	    /// This is a convenience function. Use FixtureDef if you need to set parameters
	    /// like friction, restitution, user data, or filtering.
	    /// If the density is non-zero, this function automatically updates the mass of the body.
	    /// @param shape the shape to be cloned.
	    /// @param density the shape density (set to zero for static bodies).
	    /// @warning This function is locked during callbacks.
	    public Fixture CreateFixture(Shape shape, float density)
        {
            FixtureDef def = new FixtureDef();
            def.shape = shape;
            def.density = density;

            return CreateFixture(def);
        }

	    /// Destroy a fixture. This removes the fixture from the broad-phase and
	    /// destroys all contacts associated with this fixture. This will	
        /// automatically adjust the mass of the body if the body is dynamic and the
	    /// fixture has positive density.
	    /// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
	    /// @param fixture the fixture to be removed.
	    /// @warning This function is locked during callbacks.
	    public void DestroyFixture(Fixture fixture)
        {
            Debug.Assert(_world.IsLocked == false);
	        if (_world.IsLocked == true)
	        {
		        return;
	        }

	        Debug.Assert(fixture._body == this);

	        // Remove the fixture from this body's singly linked list.
	        Debug.Assert(_fixtureCount > 0);
	        Fixture node = _fixtureList;
	        bool found = false;
	        while (node != null)
	        {
		        if (node == fixture)
		        {
                    _fixtureList = fixture._next;
			        found = true;
			        break;
		        }

		        node = node._next;
	        }

	        // You tried to remove a shape that is not attached to this body.
	        Debug.Assert(found);

	        // Destroy any contacts associated with the fixture.
	        ContactEdge edge = _contactList;
	        while (edge != null)
	        {
		        Contact c = edge.Contact;
		        edge = edge.Next;

		        Fixture fixtureA = c.GetFixtureA();
		        Fixture fixtureB = c.GetFixtureB();

		        if (fixture == fixtureA || fixture == fixtureB)
		        {
			        // This destroys the contact and removes it from
			        // this body's contact list.
			        _world._contactManager.Destroy(c);
		        }
	        }

            if ((_flags & BodyFlags.Active) == BodyFlags.Active)
            {
                Debug.Assert(fixture._proxyId != BroadPhase.NullProxy);

                BroadPhase broadPhase = _world._contactManager._broadPhase;
                fixture.DestroyProxies(broadPhase);
            }

	        fixture.Destroy();
	        fixture._body = null;
	        fixture._next = null;
	        
            --_fixtureCount;

            
            ResetMassData();
        }

	    /// Set the position of the body's origin and rotation.
	    /// This breaks any contacts and wakes the other bodies.
        /// Manipulating a body's transform may cause non-physical behavior.
	    /// @param position the world position of the body's local origin.
	    /// @param angle the world rotation in radians.
	    public void SetTransform(Vector2 position, float angle)
        {
            Debug.Assert(_world.IsLocked == false);
	        if (_world.IsLocked == true)
	        {
		        return;
	        }

	        _xf.R.Set(angle);
	        _xf.Position = position;

	        _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);
	        _sweep.a0 = _sweep.a = angle;

	        BroadPhase broadPhase = _world._contactManager._broadPhase;
	        for (Fixture f = _fixtureList; f != null; f = f._next)
	        {
		        f.Synchronize(broadPhase, ref _xf, ref _xf);
	        }

	        _world._contactManager.FindNewContacts();
        }

        // For teleporting a body without considering new contacts immediately.
        public void SetTransformIgnoreContacts(Vector2 position, float angle)
        {
            Debug.Assert(_world.IsLocked == false);
            if (_world.IsLocked == true)
            {
                return;
            }

            _xf.R.Set(angle);
            _xf.Position = position;

            _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);
            _sweep.a0 = _sweep.a = angle;

            BroadPhase broadPhase = _world._contactManager._broadPhase;
            for (Fixture f = _fixtureList; f != null; f = f._next)
            {
                f.Synchronize(broadPhase, ref _xf, ref _xf);
            }
        }

	    /// Get the body transform for the body's origin.
	    /// @return the world transform of the body's origin.
	    public void GetTransform(out Transform xf)
        {
            xf = _xf;
        }

	    /// Get the world body origin position.
	    /// @return the world position of the body's origin.
	    public Vector2 GetPosition()
        {
            return _xf.Position;
        }

	    /// Get the angle in radians.
	    /// @return the current world rotation angle in radians.
	    public float GetAngle() 
        {
            return _sweep.a;
        }

	    /// Get the world position of the center of mass.
	    public Vector2 GetWorldCenter() 
        {
            return _sweep.c;
        }

	    /// Get the local position of the center of mass.
	    public Vector2 GetLocalCenter() 
        {
            return _sweep.localCenter;
        }

	    /// Set the linear velocity of the center of mass.
	    /// @param v the new linear velocity of the center of mass.
	    public void SetLinearVelocity(Vector2 v)
        {
            if (_type == BodyType.Static)
            {
                return;
            }
            
            if (Vector2.Dot(v,v) > 0.0f)   
            {
                SetAwake(true);   
            }
 
            _linearVelocity = v;
        }

	    /// Get the linear velocity of the center of mass.
	    /// @return the linear velocity of the center of mass.
	    public Vector2 GetLinearVelocity()
        {
            return _linearVelocity;
        }

	    /// Set the angular velocity.
	    /// @param omega the new angular velocity in radians/second.
	    public void SetAngularVelocity(float w)
        {
            if (_type == BodyType.Static)
            {
                return;
            }

            if (w*w > 0.0f)
            {
                SetAwake(true);
            }

            _angularVelocity = w;
        }

	    /// Get the angular velocity.
	    /// @return the angular velocity in radians/second.
	    public float GetAngularVelocity()
        {
            return _angularVelocity;
        }

	    /// Apply a force at a world point. If the force is not
	    /// applied at the center of mass, it will generate a torque and
	    /// affect the angular velocity. This wakes up the body.
	    /// @param force the world force vector, usually in Newtons (N).
	    /// @param point the world position of the point of application.
	    public void ApplyForce(Vector2 force, Vector2 point)
        {
            if (_type == BodyType.Dynamic)
            {
                if (IsAwake() == false)
                {
                    SetAwake(true);
                }

                _force += force;
                _torque += MathUtils.Cross(point - _sweep.c, force);
            }
        }

	    /// Apply a torque. This affects the angular velocity
	    /// without affecting the linear velocity of the center of mass.
	    /// This wakes up the body.
	    /// @param torque about the z-axis (out of the screen), usually in N-m.
	    public void ApplyTorque(float torque)
        {
            if (_type == BodyType.Dynamic)
            {
                if (IsAwake() == false)
                {
                    SetAwake(true);
                }

                _torque += torque;
            }
        }

	    /// Apply an impulse at a point. This immediately modifies the velocity.
	    /// It also modifies the angular velocity if the point of application
	    /// is not at the center of mass. This wakes up the body.
	    /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
	    /// @param point the world position of the point of application.
	    public void ApplyLinearImpulse(Vector2 impulse, Vector2 point)
        {
            if (_type != BodyType.Dynamic)
            {
                return;
            }
            if (IsAwake() == false)
            {
                SetAwake(true);
            }
            _linearVelocity += _invMass * impulse;
            _angularVelocity += _invI * MathUtils.Cross(point - _sweep.c, impulse);
        }

        /// Apply an angular impulse.  
        /// @param impulse the angular impulse in units of kg*m*m/s  
        public void ApplyAngularImpulse(float impulse)
        {
            if (_type != BodyType.Dynamic)
            {
                return;
            }

            if (IsAwake() == false)
            {
                SetAwake(true);
            }

            _angularVelocity += _invI * impulse;
        }

	    /// Get the total mass of the body.
	    /// @return the mass, usually in kilograms (kg).
	    public float GetMass()
        {
            return _mass;
        }

        /// Get the rotational inertia of the body about the local origin.
	    /// @return the rotational inertia, usually in kg-m^2.
	    public float GetInertia()
        {
            return _I + _mass * Vector2.Dot(_sweep.localCenter, _sweep.localCenter);
        }

        /// Get the mass data of the body.
	    /// @return a struct containing the mass, inertia and center of the body.
	    public void GetMassData(out MassData massData)
        {
            massData = new MassData();
            massData.mass = _mass;
            massData.I = _I + _mass * Vector2.Dot(_sweep.localCenter, _sweep.localCenter);
            massData.center = _sweep.localCenter;
        }

        /// Set the mass properties to override the mass properties of the fixtures.
 	    /// Note that this changes the center of mass position.
        /// Note that creating or destroying fixtures can also alter the mass.
        /// This function has no effect if the body isn't dynamic.
        /// @param massData the mass properties.
        public void SetMassData(ref MassData massData)
        {
	        Debug.Assert(_world.IsLocked == false);
	        if (_world.IsLocked == true)
	        {
		        return;
	        }

            if (_type != BodyType.Dynamic)
            {
                return;
            }

	        _invMass = 0.0f;
	        _I = 0.0f;
	        _invI = 0.0f;

	        _mass = massData.mass;

	        if (_mass <= 0.0f)
	        {
                _mass = 1.0f;
	        }

            _invMass = 1.0f / _mass;


	        if (massData.I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
	        {
		        _I = massData.I - _mass * Vector2.Dot(massData.center, massData.center);
                Debug.Assert(_I > 0.0f);
		        _invI = 1.0f / _I;
	        }

	        // Move center of mass.
	        Vector2 oldCenter = _sweep.c;
	        _sweep.localCenter = massData.center;
	        _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);

	        // Update center of mass velocity.
            _linearVelocity += MathUtils.Cross(_angularVelocity, _sweep.c - oldCenter);
        }

        /// This resets the mass properties to the sum of the mass properties of the fixtures.
        /// This normally does not need to be called unless you called SetMassData to override
        /// the mass and you later want to reset the mass.
        public void ResetMassData()
        {
	        // Compute mass data from shapes. Each shape has its own density.
	        _mass = 0.0f;
	        _invMass = 0.0f;
	        _I = 0.0f;
	        _invI = 0.0f;
            _sweep.localCenter = Vector2.Zero;

            // Static and kinematic bodies have zero mass.
            if (_type == BodyType.Static || _type == BodyType.Kinematic)
            {
                _sweep.c0 = _sweep.c = _xf.Position;
                return;
            }

            Debug.Assert(_type == BodyType.Dynamic);

            // Accumulate mass over all fixtures.
	        Vector2 center = Vector2.Zero;
	        for (Fixture f = _fixtureList; f != null; f = f._next)
	        {
                if (f._density == 0.0f)
                {
                    continue;
                }

                MassData massData;
                f.GetMassData(out massData);
		        _mass += massData.mass;
		        center += massData.mass * massData.center;
		        _I += massData.I;
	        }

	        // Compute center of mass.
	        if (_mass > 0.0f)
	        {
		        _invMass = 1.0f / _mass;
		        center *= _invMass;
	        }
            else
            {
                // Force all dynamic bodies to have a positive mass.
                _mass = 1.0f;
                _invMass = 1.0f;
            }

	        if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
	        {
		        // Center the inertia about the center of mass.
		        _I -= _mass * Vector2.Dot(center, center);

		        Debug.Assert(_I > 0.0f);
		        _invI = 1.0f / _I;
	        }
	        else
	        {
		        _I = 0.0f;
		        _invI = 0.0f;
	        }

	        // Move center of mass.
	        Vector2 oldCenter = _sweep.c;
	        _sweep.localCenter = center;
	        _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);

	        // Update center of mass velocity.
	        _linearVelocity += MathUtils.Cross(_angularVelocity, _sweep.c - oldCenter);
        }

	    /// Get the world coordinates of a point given the local coordinates.
	    /// @param localPoint a point on the body measured relative the the body's origin.
	    /// @return the same point expressed in world coordinates.
	    public Vector2 GetWorldPoint(Vector2 localPoint)
        {
            return MathUtils.Multiply(ref _xf, localPoint);
        }

	    /// Get the world coordinates of a vector given the local coordinates.
	    /// @param localVector a vector fixed in the body.
	    /// @return the same vector expressed in world coordinates.
	    public Vector2 GetWorldVector(Vector2 localVector)
        {
            return MathUtils.Multiply(ref _xf.R, localVector);
        }

	    /// Gets a local point relative to the body's origin given a world point.
	    /// @param a point in world coordinates.
	    /// @return the corresponding local point relative to the body's origin.
	    public Vector2 GetLocalPoint(Vector2 worldPoint)
        {
            return MathUtils.MultiplyT(ref _xf, worldPoint);
        }

	    /// Gets a local vector given a world vector.
	    /// @param a vector in world coordinates.
	    /// @return the corresponding local vector.
	    public Vector2 GetLocalVector(Vector2 worldVector)
        {
            return MathUtils.MultiplyT(ref _xf.R, worldVector);
        }

	    /// Get the world linear velocity of a world point attached to this body.
	    /// @param a point in world coordinates.
	    /// @return the world velocity of a point.
	    public Vector2 GetLinearVelocityFromWorldPoint(Vector2 worldPoint)
        {
            return _linearVelocity + MathUtils.Cross(_angularVelocity, worldPoint - _sweep.c);
        }

	    /// Get the world velocity of a local point.
	    /// @param a point in local coordinates.
	    /// @return the world velocity of a point.
	    public Vector2 GetLinearVelocityFromLocalPoint(Vector2 localPoint)
        {
            return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
        }

	    /// Get the linear damping of the body.
	    public float GetLinearDamping()
        {
            return _linearDamping;
        }

	    /// Set the linear damping of the body.
	    public void SetLinearDamping(float linearDamping)
        {
            _linearDamping = linearDamping;
        }

	    /// Get the angular damping of the body.
	    public float GetAngularDamping()
        {
            return _angularDamping;
        }

	    /// Set the angular damping of the body.
	    public void SetAngularDamping(float angularDamping)
        {
            _angularDamping = angularDamping;
        }

	    /// Is this body treated like a bullet for continuous collision detection?
	    public bool IsBullet
        {
            get
            {
                return (_flags & BodyFlags.Bullet) == BodyFlags.Bullet;
            }
        }

	    /// Should this body be treated like a bullet for continuous collision detection?
	    public void SetBullet(bool flag)
        {
            if (flag)
	        {
		        _flags |= BodyFlags.Bullet;
	        }
	        else
	        {
		        _flags &= ~BodyFlags.Bullet;
	        }
        }

        /// You can disable sleeping on this body. If you disable sleeping, the
	    /// body will be woken.
	    void SetSleepingAllowed(bool flag)
        {
            if (flag)
            {
                _flags |= BodyFlags.AutoSleep;
            }
            else
            {
                _flags &= ~BodyFlags.AutoSleep;
                SetAwake(true);
            }

        }

	    /// Is this body allowed to sleep
	    public bool IsSleepingAllowed
        {
            get
            {
                return (_flags & BodyFlags.AutoSleep) == BodyFlags.AutoSleep;
            }
        }

	    /// You can disable sleeping on this body.
	    public void AllowSleeping(bool flag)
        {
            if (flag)
	        {
		        _flags |= BodyFlags.AutoSleep;
	        }
	        else
	        {
		        _flags &= ~BodyFlags.AutoSleep;
		        SetAwake(true);
	        }
        }

	    /// Set the sleep state of the body. A sleeping body has very
	    /// low CPU cost.
	    /// @param flag set to true to put body to sleep, false to wake it.
	    public void SetAwake(bool flag)
        {
            if (flag)
            {
                if ((_flags & BodyFlags.Awake) == 0)
                {
                    _flags |= BodyFlags.Awake;
                    _sleepTime = 0.0f;
                }
            }
            else
            {
                _flags &= ~BodyFlags.Awake;
                _sleepTime = 0.0f;
                _linearVelocity = Vector2.Zero;
                _angularVelocity = 0.0f;
                _force = Vector2.Zero;
                _torque = 0.0f;
            }
        }

        /// Get the sleeping state of this body.
	    /// @return true if the body is sleeping.
	    public bool IsAwake()
        {
            return (_flags & BodyFlags.Awake) == BodyFlags.Awake;
        }

	    /// Set the active state of the body. An inactive body is not
	    /// simulated and cannot be collided with or woken up.
	    /// If you pass a flag of true, all fixtures will be added to the
	    /// broad-phase.
	    /// If you pass a flag of false, all fixtures will be removed from
	    /// the broad-phase and all contacts will be destroyed.
	    /// Fixtures and joints are otherwise unaffected. You may continue
	    /// to create/destroy fixtures and joints on inactive bodies.
	    /// Fixtures on an inactive body are implicitly inactive and will
	    /// not participate in collisions, ray-casts, or queries.
	    /// Joints connected to an inactive body are implicitly inactive.
	    /// An inactive body is still owned by a b2World object and remains
	    /// in the body list.
        public void SetActive(bool flag)
        {
            if (flag == IsActive())
            {
                return;
            }

            if (flag)
            {
                _flags |= BodyFlags.Active;

                // Create all proxies.
                BroadPhase broadPhase = _world._contactManager._broadPhase;
                for (Fixture f = _fixtureList; f != null; f = f._next)
                {
                    f.CreateProxies(broadPhase, ref _xf);
                }

                // Contacts are created the next time step.
            }
            else
            {
                _flags &= ~BodyFlags.Active;

                // Destroy all proxies.
                BroadPhase broadPhase = _world._contactManager._broadPhase;
                for (Fixture f = _fixtureList; f != null; f = f._next)
                {
                    f.DestroyProxies(broadPhase);
                }

                // Destroy the attached contacts.
                ContactEdge ce = _contactList;
                while (ce != null)
                {
                    ContactEdge ce0 = ce;
                    ce = ce.Next;
                    _world._contactManager.Destroy(ce0.Contact);
                }
                _contactList = null;
            }
        }

	    /// Get the active state of the body.
	    public bool IsActive()
        {
            return (_flags & BodyFlags.Active) == BodyFlags.Active;
        }

	    /// Set this body to have fixed rotation. This causes the mass
	    /// to be reset.
        public void SetFixedRotation(bool flag)
        {
            if (flag)
            {
                _flags |= BodyFlags.FixedRotation;
            }
            else
            {
                _flags &= ~BodyFlags.FixedRotation;
            }

            ResetMassData();
        }

	    /// Does this body have fixed rotation?
        public bool IsFixedRotation()
        {
            return (_flags & BodyFlags.FixedRotation) == BodyFlags.FixedRotation;
        }

	    public Fixture GetFixtureList()
        {
            return _fixtureList;
        }

	    /// Get the list of all joints attached to this body.
	    public JointEdge GetJointList()
        {
            return _jointList;
        }

	    /// Get the list of all contacts attached to this body.
	    /// @warning this list changes during the time step and you may
	    /// miss some collisions if you don't use ContactListener.
	    public ContactEdge GetContactList()
        {
            return _contactList;
        }

        /// Get the next body in the world's body list.
	    public Body GetNext()
        {
            return _next;
        }

	    /// Get the user data pointer that was provided in the body definition.
	    public object GetUserData()
        {
            return _userData;
        }

	    /// Set the user data. Use this to store your application specific data.
	    public void SetUserData(object data)
        {
            _userData = data;
        }

	    /// Get the parent world of this body.
	    public World GetWorld()
        {
            return _world;
        }

        internal Body(BodyDef bd, World world)
        {
            Debug.Assert(bd.position.IsValid());
            Debug.Assert(bd.linearVelocity.IsValid());
            Debug.Assert(MathUtils.IsValid(bd.angle));
            Debug.Assert(MathUtils.IsValid(bd.angularVelocity));
            Debug.Assert(MathUtils.IsValid(bd.inertiaScale) && bd.inertiaScale >= 0.0f);
            Debug.Assert(MathUtils.IsValid(bd.angularDamping) && bd.angularDamping >= 0.0f);
            Debug.Assert(MathUtils.IsValid(bd.linearDamping) && bd.linearDamping >= 0.0f);


	        if (bd.bullet)
	        {
		        _flags |= BodyFlags.Bullet;
	        }
	        if (bd.fixedRotation)
	        {
		        _flags |= BodyFlags.FixedRotation;
	        }
	        if (bd.allowSleep)
	        {
		        _flags |= BodyFlags.AutoSleep;
	        }
            if (bd.awake)
            {
                _flags |= BodyFlags.Awake;
            }
            if (bd.active)
            {
                _flags |= BodyFlags.Active;
            }

	        _world = world;

	        _xf.Position = bd.position;
	        _xf.R.Set(bd.angle);

	        _sweep.a0 = _sweep.a = bd.angle;
	        _sweep.c0 = _sweep.c = MathUtils.Multiply(ref _xf, _sweep.localCenter);

	        _linearVelocity = bd.linearVelocity;
	        _angularVelocity = bd.angularVelocity;

	        _linearDamping = bd.linearDamping;
	        _angularDamping = bd.angularDamping;
            
            _type = bd.type;
            if (_type == BodyType.Dynamic)
            {
                _mass = 1.0f;
                _invMass = 1.0f;
            }

	        _userData = bd.userData;
        }

        internal void SynchronizeFixtures()
        {
            Transform xf1 = new Transform();
	        xf1.R.Set(_sweep.a0);
	        xf1.Position = _sweep.c0 - MathUtils.Multiply(ref xf1.R, _sweep.localCenter);

	        BroadPhase broadPhase = _world._contactManager._broadPhase;
	        for (Fixture f = _fixtureList; f != null; f = f._next)
	        {
		        f.Synchronize(broadPhase, ref xf1, ref _xf);
	        }
        }

	    internal void SynchronizeTransform()
        {
            _xf.R.Set(_sweep.a);
	        _xf.Position = _sweep.c - MathUtils.Multiply(ref _xf.R, _sweep.localCenter);
        }

	    // This is used to prevent connected bodies from colliding.
	    // It may lie, depending on the collideConnected flag.
	    internal bool ShouldCollide(Body other)
        {	
            // At least one body should be dynamic.
            if (_type != BodyType.Dynamic && other._type != BodyType.Dynamic)
            {
                return false;
            }

            // Does a joint prevent collision?
            for (JointEdge jn = _jointList; jn != null; jn = jn.Next)
	        {
		        if (jn.Other == other)
		        {
                    if (jn.Joint._collideConnected == false)
                    {
                        return false;
                    }
		        }
	        }

	        return true;
        }

	    internal void Advance(float t)
        {
            // Advance to the new safe time.
	        _sweep.Advance(t);
	        _sweep.c = _sweep.c0;
	        _sweep.a = _sweep.a0;
	        SynchronizeTransform();
        }

        /// <summary>
        /// Get the world body origin position.
        /// </summary>
        /// <returns>Return the world position of the body's origin.</returns>
        public Vector2 Position
        {
            get
            {
                return _xf.Position;
            }
            set
            {
                SetTransform(value, Rotation);
            }
        }

        /// <summary>
        /// Get the angle in radians.
        /// </summary>
        /// <returns>Return the current world rotation angle in radians.</returns>
        public float Rotation
        {
            get
            {
                return _sweep.a;
            }
            set
            {
                SetTransform(Position, value);
            }
        }

        internal BodyFlags _flags;
        internal BodyType _type;

        internal int _islandIndex;

        internal Transform _xf;		// the body origin transform
        internal Sweep _sweep;	// the swept motion for CCD

        internal Vector2 _linearVelocity;
        internal float _angularVelocity;

        internal Vector2 _force;
        internal float _torque;

        internal World _world;
        internal Body _prev;
        internal Body _next;

        internal Fixture _fixtureList;
        internal int _fixtureCount;

        internal JointEdge _jointList;
        internal ContactEdge _contactList;

        internal float _mass, _invMass;

        // Rotational inertia about the center of mass.
        internal float _I, _invI;

        internal float _linearDamping;
        internal float _angularDamping;

        internal float _sleepTime;

        internal object _userData;

        internal float _intertiaScale;
    }
}