/// <summary>
		/// Construct a new world point constraint.
		/// </summary>
		/// <param name="body">The body to constrain.</param>
		/// <param name="worldPoint">The point in world coordinates to constrain the point to. The body is expected to be occupying this
		/// point at the time the constraint is created.</param>
		public WorldPointConstraint(RigidBody body, Vector3 worldPoint)
			: base(body, body)
		{
			_worldPoint = worldPoint;
			Vector3.Transform(ref worldPoint, ref BodyA.WorldInverse.Combined, out _bodyPoint);
            orientation = body.Orientation;
		}
Пример #2
0
		/// <summary>
		/// Construct a new constraint.
		/// </summary>
		/// <param name="bodyA">The first constrained body.</param>
		/// <param name="bodyB">The second constrained body.</param>
		public Constraint(RigidBody bodyA, RigidBody bodyB)
		{
			_bodyA = bodyA;
			_bodyB = bodyB;

			this.ConstraintId = Interlocked.Increment(ref CurrentConstraintId);
		}
		/// <summary>
		/// Construct a new distance constraint.
		/// </summary>
		/// <param name="bodyA">The first body.</param>
		/// <param name="bodyB">The second body.</param>
		/// <param name="worldPointA">The world-space point at which the constraint is applied on the first body.</param>
		/// <param name="worldPointB">The world-space point at which the constraint is applied on the second body.</param>
		/// <param name="minDistance">The minimum allowed distance.</param>
		/// <param name="maxDistance">The maximum allowed distance.</param>
		public DistanceConstraint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPointA, Vector3 worldPointB, float minDistance, float maxDistance)
			: base(bodyA, bodyB)
		{
			// store body points
			Vector3.Transform(ref worldPointA, ref bodyA.WorldInverse.Combined, out _bodyPointA);
			Vector3.Transform(ref worldPointB, ref bodyB.WorldInverse.Combined, out _bodyPointB);
			_minDistance = minDistance;
			_maxDistance = maxDistance;
		}
Пример #4
0
 protected bool onCollide(RigidBody r1, RigidBody r2)
 {
     if (r2 == room.TrackedObject)
     {
         room.TrackedObject = r1 as ITrackable;
         this.OnCollision = null;
     }
     return false;
 }
Пример #5
0
		/// <summary>
		/// Construct a new spring force.
		/// </summary>
		/// <param name="bodyA">The first body on which force is applied.</param>
		/// <param name="bodyB">The second body on which force is applied.</param>
		/// <param name="bodyPointA">The point in local-space on the first body on which to apply force.</param>
		/// <param name="bodyPointB">The point in local-space on the second body on which to apply force.</param>
		/// <param name="length">The resting length of the spring.</param>
		/// <param name="stiffness">The stiffness factor of the spring.</param>
		/// <param name="damping">The damping coefficient.</param>
		public SpringForce(RigidBody bodyA, RigidBody bodyB, Vector3 bodyPointA, Vector3 bodyPointB, float length, float stiffness, float damping)
		{
			_bodyA = bodyA;
			_bodyB = bodyB;
			_bodyPointA = bodyPointA;
			_bodyPointB = bodyPointB;
			_length = length;
			_k = stiffness;
			_c = damping;
		}
Пример #6
0
		/// <summary>
		/// Construct a new generic constraint.
		/// </summary>
		/// <param name="bodyA">The first constrained body.</param>
		/// <param name="bodyB">The second constrained body.</param>
		/// <param name="basis">The world-space frame of the second body against which the first body will be constrained. This value
		/// must contain the initial position and orientation of the second body at the time the constraint was created.</param>
		/// <param name="limitedPosAxes">Specifies which axes around which to constrain position.</param>
		/// <param name="minPos">The minimum distances between the two bodies along each axis.</param>
		/// <param name="maxPos">The maximum distances between the two bodies along each axis.</param>
		/// <param name="limitedRotAxes">Specifies which axes around which to constrain rotation.</param>
		/// <param name="minAngles">The minimum relative angles, in radians, around each axis.</param>
		/// <param name="maxAngles">The maximum relative angles, in radians, around each axis.</param>
		public GenericConstraint(RigidBody bodyA, RigidBody bodyB, Frame basis, Axes limitedPosAxes,
			Vector3 minPos, Vector3 maxPos, Axes limitedRotAxes, Vector3 minAngles, Vector3 maxAngles)
			: base(bodyA, bodyB)
		{
			Frame.Transform(ref basis, ref bodyA.WorldInverse, out _bodyBasisA);
			Frame.Transform(ref basis, ref bodyB.WorldInverse, out _bodyBasisB);
			_limitedPosAxes = limitedPosAxes;
			_limitedRotAxes = limitedRotAxes;
			_minPos = minPos;
			_maxPos = maxPos;
			_minAngles = minAngles;
			_maxAngles = maxAngles;
		}
Пример #7
0
		/// <summary>
		/// Construct a new universal joint.
		/// </summary>
		/// <param name="bodyA">The first body to constrain.</param>
		/// <param name="bodyB">The second body to constrain.</param>
		/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
		/// <param name="axisA">The axis fixed to the first body which the second body can rotate around.</param>
		/// <param name="axisB">The axis fixed to the second body which the first body can rotate around.</param>
		/// <param name="aMin">The minimum angle that the second body must maintain relative to the first around the first body's axis.
		/// This number should be zero or negative.</param>
		/// <param name="aMax">The maximum angle that the second body must maintain relative to the first around the first body's axis.
		/// This number should be zero or positive.</param>
		/// <param name="bMin">The minimum angle that the first body must maintain relative to the second around the second body's axis.
		/// This number should be zero or negative.</param>
		/// <param name="bMax">The maximum angle that the first body must maintain relative to the second around the second body's axis.
		/// This number should be zero or positive.</param>
		public UniversalJoint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint, Vector3 axisA, Vector3 axisB,
			float aMin, float aMax, float bMin, float bMax)
			: base(bodyA, bodyB)
		{
			aMin = MathHelper.Clamp(aMin, -MathHelper.PiOver2 + Constants.Epsilon, 0f);
			aMax = MathHelper.Clamp(aMax, 0f, MathHelper.PiOver2 - Constants.Epsilon);
			bMin = MathHelper.Clamp(bMin, -MathHelper.PiOver2 + Constants.Epsilon, 0f);
			bMax = MathHelper.Clamp(bMax, 0f, MathHelper.PiOver2 - Constants.Epsilon);

			var frame = new Frame(Vector3.Zero, axisA, axisB, worldPoint);

			this.Constraints.Add(new PointConstraint(bodyA, bodyB, worldPoint));
			this.Constraints.Add(new GenericConstraint(bodyA, bodyB, frame,
				Axes.None, Vector3.Zero, Vector3.Zero,
				Axes.All, new Vector3(0f, aMin, bMin), new Vector3(0f, aMax, bMax)));
		}
Пример #8
0
		/// <summary>
		/// Construct a new revolute joint.
		/// </summary>
		/// <param name="bodyA">The first constrained body.</param>
		/// <param name="bodyB">The second constrained body.</param>
		/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
		/// <param name="axis">The axis around which to allow relative rotation. Rotation around the other axes will be prevented.</param>
		/// <param name="minAngle">The minimum angle specifies (in radians) the most that the first body can rotate clockwise relative
		/// to the second body around the axis. This number should be zero or negative.</param>
		/// <param name="maxAngle">The maximum angle specifies (in radians) the most that the first body can rotate counter-clockwise relative
		/// to the second body around the axis. This number should be zero or positive.</param>
		public RevoluteJoint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint, Vector3 axis, float minAngle, float maxAngle)
			: base(bodyA, bodyB)
		{
			var frame = new Frame(worldPoint);

			// create an arbitrary basis with the axis as X
			Vector3 n = Vector3.UnitZ;
			Vector3.Cross(ref n, ref axis, out n);
			frame.X = axis;
			if (n.LengthSquared() < Constants.Epsilon)
			{
				n = Vector3.UnitY;
				Vector3.Cross(ref axis, ref n, out n);
				frame.Z = n;
			}
			else
				frame.Y = n;
			frame.Normalize();

			this.Constraints.Add(new GenericConstraint(bodyA, bodyB, frame,
				Axes.All, Vector3.Zero, Vector3.Zero,
				Axes.All, new Vector3(minAngle, 0f, 0f), new Vector3(maxAngle, 0f, 0f)));
		}
Пример #9
0
        public void processTouchPoints(ReadOnlyTouchPointCollection touches)
        {
            lastTouchPosition = touchPosition;

            int tagID = -1;
            if (touches.Count >= 1)
            {
                for (int i = 0; i < touches.Count; i++)
                {
                    if (touches[i].IsTagRecognized)
                    {
                        tagID = (int)touches[i].Tag.Value;
                        break;
                    }
                }
                if (tagID != -1)
                {
                    touchPosition = touches[0];
                    //First time touch
                    if (lastTouchPosition == null)
                    {
                        Segment s;
                        s.P1 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.CenterX, touchPosition.CenterY, 0f),
                            viewManager.Projection, viewManager.View, Matrix.Identity);
                        s.P2 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.CenterX, touchPosition.CenterY, 1f),
                            viewManager.Projection, viewManager.View, Matrix.Identity);
                        float scalar;
                        Vector3 point;
                        var c = physics.BroadPhase.Intersect(ref s, out scalar, out point);

                        if (c != null && c is BodySkin)
                        {
                            pickedObject = ((BodySkin)c).Owner;

                            pickedForce = new WorldPointConstraint(pickedObject, point);
                            physics.Add(pickedForce);
                            pickedDistance = scalar;
                            pickedObject.IsActive = true;
                        }
                        lastOrientation = touches.Count == 1 ? touches[0].Orientation : touches[1].Orientation;
                    }
                    else if (pickedObject != null)
                    {
                        Segment s;
                        s.P1 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.CenterX, touchPosition.CenterY, 0f),
                            viewManager.Projection, viewManager.View, Matrix.Identity);
                        s.P2 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.CenterX, touchPosition.CenterY, 1f),
                            viewManager.Projection, viewManager.View, Matrix.Identity);
                        Vector3 diff, point;
                        Vector3.Subtract(ref s.P2, ref s.P1, out diff);
                        Vector3.Multiply(ref diff, pickedDistance, out diff);
                        Vector3.Add(ref s.P1, ref diff, out point);
                        pickedForce.WorldPoint = point;
                        pickedObject.IsActive = true;

                        //SolidThing po = (SolidThing)pickedObject;
                        //Console.Out.WriteLine("UNFREEEEEEEEEEEEZE");
                        switch (tagID)
                        {
                            
                            //Pin a block
                            case 0:
                                pickedObject.Freeze();
                                break;
                            //unPin a block
                            case 1:
                                
                                pickedObject.Unfreeze();
                                break;
                            //Rotate a block
                            case 2:
                                pickedForce.orientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, -1.0f), touchPosition.Orientation);
                                break;
                            //Move a block towards or away from camera
                            case 3:
                                TouchPoint tagPoint = touches[0];
                                float deltaRotation = MathHelper.ToDegrees(lastOrientation) - MathHelper.ToDegrees(tagPoint.Orientation);

                                Vector3 direction = new Vector3(0, 0, 1.0f);
                                direction.Normalize();
                                pickedForce.WorldPoint = Vector3.Add(pickedForce.WorldPoint, Vector3.Multiply(direction, deltaRotation * 0.01f));

                                break;
                            //Rotate stack onto top view
                            /*case 4:
                                viewManager.rotateToSide(4, touch);
                                break;
                            //Corkscrew closer or further away
                            case 5:
                                viewManager.rotateToSide(5);
                                break;
                            case 6:
                                viewManager.rotateToSide(6);
                                break;
                            case 7:
                                viewManager.rotateToSide(7);
                                break;
                            case 8:
                                viewManager.rotateToSide(8);
                                break;
                                */

                        }
                    }
                    else if (pickedObject != null)
                    {
                        physics.Remove(pickedForce);
                        pickedObject = null;
                    }
                }
            }
            else if (pickedObject != null)
            {
                physics.Remove(pickedForce);
                pickedObject = null;
                touchPosition = null;
                lastTouchPosition = null;
            }
            else
            {
                touchPosition = null;
            }

        }
Пример #10
0
        protected override void Update(GameTime gameTime)
        {
            _inputManager.CaptureMouse = this.IsActive && _inputManager.MouseState.RightButton == ButtonState.Pressed;
            #if WINDOWS
            _physics.Enabled = !Program.Proxy.IsPaused;
            #endif

            #if WINDOWS || WINDOWS_PHONE
            foreach (Keys k in _inputManager.KeysPressed)
            {
                switch (k)
                {
                    case Keys.Space:
                        SpawnSelectedObject();
                        break;
                    case Keys.R:
                        SpawnRagdoll();
                        break;
                    case Keys.N:
                        if (!_physics.Enabled)
                        {
                            _physics.Integrate((float)gameTime.ElapsedGameTime.TotalSeconds);
                        }
                        break;
                    case Keys.D0:
                    case Keys.D1:
                    case Keys.D2:
                    case Keys.D3:
                    case Keys.D4:
                    case Keys.D5:
                    case Keys.D6:
                    case Keys.D7:
                    case Keys.D8:
                    case Keys.D9:
                        CreateScene(k - Keys.D0);
                        break;
                    case Keys.Escape:
                    case Keys.Q:
                        this.Exit();
                        break;
                }
            }

            // object picking
            if (_inputManager.WasPressed(MouseButton.MiddleButton))
            {
                Segment s;
                s.P1 = GraphicsDevice.Viewport.Unproject(new Vector3(_inputManager.MouseState.X, _inputManager.MouseState.Y, 0f),
                    _viewManager.Projection, _viewManager.View, Matrix.Identity);
                s.P2 = GraphicsDevice.Viewport.Unproject(new Vector3(_inputManager.MouseState.X, _inputManager.MouseState.Y, 1f),
                    _viewManager.Projection, _viewManager.View, Matrix.Identity);
                float scalar;
                Vector3 point;
                var c = _physics.BroadPhase.Intersect(ref s, out scalar, out point);
                if (c != null && c is BodySkin)
                {
                    _pickedObject = ((BodySkin)c).Owner;

                    _pickedForce = new GrabConstraint(_pickedObject, point);
                    _physics.Add(_pickedForce);
                    _pickedDistance = scalar;
                    _pickedObject.IsActive = true;
                }
            }
            else if (_inputManager.MouseState.MiddleButton == ButtonState.Pressed && _pickedObject != null)
            {
                Segment s;
                s.P1 = GraphicsDevice.Viewport.Unproject(new Vector3(_inputManager.MouseState.X, _inputManager.MouseState.Y, 0f),
                    _viewManager.Projection, _viewManager.View, Matrix.Identity);
                s.P2 = GraphicsDevice.Viewport.Unproject(new Vector3(_inputManager.MouseState.X, _inputManager.MouseState.Y, 1f),
                    _viewManager.Projection, _viewManager.View, Matrix.Identity);
                Vector3 diff, point;
                Vector3.Subtract(ref s.P2, ref s.P1, out diff);
                Vector3.Multiply(ref diff, _pickedDistance, out diff);
                Vector3.Add(ref s.P1, ref diff, out point);
                _pickedForce.WorldPoint = point;
                _pickedObject.IsActive = true;
            }
            else if (_pickedObject != null)
            {
                _physics.Remove(_pickedForce);
                _pickedObject = null;
            }
            #else
            if (_inputManager.WasPressed(Buttons.RightShoulder)) CreateScene(++curScene);
            if (_inputManager.WasPressed(Buttons.LeftShoulder)) CreateScene(curScene == 0 ? 0 : --curScene);
            if (_inputManager.WasPressed(Buttons.A)) SpawnSelectedObject();
            if (_inputManager.WasPressed(Buttons.B)) SpawnRagdoll();
            if (_inputManager.WasPressed(Buttons.X))
                _physics.Enabled = !_physics.Enabled;
            if (_inputManager.WasPressed(Buttons.Y)) _physics.Integrate((float)gameTime.ElapsedGameTime.TotalSeconds);
            if (_inputManager.WasPressed(Buttons.Back)) this.Exit();
            #endif
            UpdateFps(gameTime);

            base.Update(gameTime);
        }
Пример #11
0
		/// <summary>
		/// Construct a new revolute joint.
		/// </summary>
		/// <param name="bodyA">The first constrained body.</param>
		/// <param name="bodyB">The second constrained body.</param>
		/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
		/// <param name="axis">The axis around which to allow relative rotation. Rotation around the other axes will be prevented.</param>
		public RevoluteJoint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint, Vector3 axis)
			: this(bodyA, bodyB, worldPoint, axis, -MathHelper.PiOver2, MathHelper.PiOver2)
		{
		}
		private void Activate(RigidBody body)
		{
			body.IsActive = true;
			for (int i = 0; i < body.Constraints.Count; i++)
			{
				var c = body.Constraints[i];
				if (!c.BodyA.IsActive && c.BodyA.IsMovable) Activate(body.Constraints[i].BodyA);
				if (!c.BodyB.IsActive && c.BodyB.IsMovable) Activate(body.Constraints[i].BodyB);
			}
			for (int i = 0; i < body.Contacts.Count; i++)
			{
				var c = body.Contacts[i];
				if (!c.BodyA.IsActive && c.BodyA.IsMovable) Activate(c.BodyA);
				if (!c.BodyB.IsActive && c.BodyB.IsMovable) Activate(c.BodyB);
			}
		}
Пример #13
0
        private bool OnCollide(RigidBody b1, RigidBody b2)
        {
            Body body1 = b1 as Body;
              Body body2 = b2 as Body;

              if (body1.Tag.Equals("cup_bottom") && body2.Tag.Equals("sphere")) {
            Console.WriteLine("CONTACT");
            physicsManager.Remove(body2);
            bodies.Remove(body2);
              } else if (body1.Tag.Equals("sphere") && body2.Tag.Equals("cup_bottom")) {
            Console.WriteLine("CONTACT");
            physicsManager.Remove(body1);
            bodies.Remove(body1);
              }

              return false;
        }
Пример #14
0
        /// <summary>
        /// Remove a body from the physics world.
        /// </summary>
        /// <param name="body">The body to remove.</param>
        /// <returns>Returns a value indicating whether the body was successfully removed.</returns>
        public bool Remove(RigidBody body)
        {
            var fg = body as IForceGenerator;
            if (fg != null)
            {
                _generators.Remove(fg);
            }

            body.Manager = null;
            _broadPhase.Remove(body.Skin);
            return _bodies.Remove(body);
        }
Пример #15
0
		/// <summary>
		/// Construct a new point constraint.
		/// </summary>
		/// <param name="bodyA">The first constrained body.</param>
		/// <param name="bodyB">The second constrained body.</param>
		/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
		public PointConstraint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint)
			: base(bodyA, bodyB)
		{
			Vector3.Transform(ref worldPoint, ref BodyA.WorldInverse.Combined, out _bodyPointA);
			Vector3.Transform(ref worldPoint, ref BodyB.WorldInverse.Combined, out _bodyPointB);
		}
Пример #16
0
		/// <summary>
		/// Reset the constraint to default values.
		/// </summary>
		public virtual void Recycle()
		{
			_island = null;
			_bodyA = _bodyB = null;
		}
Пример #17
0
		/// <summary>
		/// Construct a new universal joint.
		/// </summary>
		/// <param name="bodyA">The first body to constrain.</param>
		/// <param name="bodyB">The second body to constrain.</param>
		/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
		/// <param name="axisA">The axis fixed to the first body which the second body can rotate around.</param>
		/// <param name="axisB">The axis fixed to the second body which the first body can rotate around.</param>
		public UniversalJoint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint, Vector3 axisA, Vector3 axisB)
			: this(bodyA, bodyB, worldPoint, axisA, axisB,
			-MathHelper.PiOver2 + 0.01f, MathHelper.PiOver2 - 0.01f,
			-MathHelper.PiOver2 + 0.01f, MathHelper.PiOver2 - 0.01f)
		{
		}
Пример #18
0
		/// <summary>
		/// Construct a new grab constraint.
		/// </summary>
		/// <param name="body">The body to constrain.</param>
		/// <param name="worldPoint">The initial world point to which the body should be constrained.</param>
		/// <param name="bodyPoint">The initial body point, in world coordinates, that the constraint is applied to.</param>
		public GrabConstraint(RigidBody body, Vector3 worldPoint, Vector3 bodyPoint)
			: base(body, body)
		{
			_worldPoint = worldPoint;
			_bodyPoint = bodyPoint;
		}
Пример #19
0
		/// <summary>
		/// Construct a new grab constraint.
		/// </summary>
		/// <param name="body">The body to constrain.</param>
		/// <param name="worldPoint">The initial world point to which the body should be constrained. It is expected that the body
		/// is occupying this point when the constraint is created.</param>
		public GrabConstraint(RigidBody body, Vector3 worldPoint)
			: this(body, worldPoint, Vector3.Transform(worldPoint, body.WorldInverse.Combined))
		{
		}
Пример #20
0
        /// <summary>
        /// Allows the game to perform any initialization it needs to before starting to run.
        /// This is where it can query for any required services and load any non-graphic
        /// related content.  Calling base.Initialize will enumerate through any components
        /// and initialize them as well.
        /// </summary>
        protected override void Initialize()
        {
            // TODO: Add your initialization logic here

              base.Initialize();

              physicsManager.LinearErrorTolerance = .2f;

              Model cupBottomModel = Content.Load<Model>("cup_bottom");
              Model cupTopModel = Content.Load<Model>("cup_top");

              Body cupBottom = new Body(this, cupBottomModel, "cup_bottom");
              Body cupTop = new Body(this, cupTopModel, "cup_top");

              cupBottom.OnCollision += OnCollide;
              for (int i = 0; i < cupBottom.Skin.Count; i++) {
            cupBottom.Skin.SetMaterial(cupBottom.Skin[i], new Material(1, .5f));
              }

              cupBottom.MassProperties = MassProperties.Immovable;
              cupBottom.SetWorld(10, Vector3.Zero, Quaternion.Identity);
              bodies.Add(cupBottom);

              for (int i = 0; i < cupTop.Skin.Count; i++) {
            cupTop.Skin.SetMaterial(cupTop.Skin[i], new Material(1, .5f));
              }
              cupTop.MassProperties = MassProperties.Immovable;
              cupTop.SetWorld(10, Vector3.Zero, Quaternion.Identity);

              bodies.Add(cupTop);

              BodySkin skin = new BodySkin();
              skin.DefaultMaterial = new Material(.4f, .5f);
              skin.Add(
            new PlanePart(new Vector3(0, 0, -1f), Vector3.UnitZ),
            new PlanePart(new Vector3(0, 1, 0), -Vector3.UnitY),
            new PlanePart(new Vector3(0, 0, 0), Vector3.UnitY),
            new PlanePart(new Vector3(1, 0, 0), -Vector3.UnitX),
            new PlanePart(new Vector3(-1, 0, 0), Vector3.UnitX)
              );
              wall = new RigidBody(skin);
              wall.SetWorld(10, Vector3.Zero, Quaternion.Identity);

              physicsManager.Gravity = new Vector3(0,-9.8f, 0);
              physicsManager.Add(cupBottom);
              physicsManager.Add(cupTop);
              physicsManager.Add(wall);

              prevState = Keyboard.GetState();
        }
Пример #21
0
        /// <summary>
        /// Add a body to the physics world.
        /// </summary>
        /// <param name="body">The body to add. It must not already be managed by a physics implementation.</param>
        public void Add(RigidBody body)
        {
            if (body.Manager != null)
                throw new ArgumentException("Body is already managed by a physics implementation.");
            _bodies.Add(body);
            _broadPhase.Add(body.Skin);
            body.Manager = this;

            var fg = body as IForceGenerator;
            if (fg != null)
            {
                _generators.Add(fg);
            }
        }
		private bool UpdateContactStates(RigidBody a, RigidBody b)
		{
			if (b.OnCollision != null || b.OnSeparation != null)
			{
				if (b.ContactStates == null)
				{
					b.ContactStates = new Dictionary<RigidBody, ContactStateFlags>();
				}
				if (!b.ContactStates.ContainsKey(a))
				{
					b.ContactStates.Add(a, 0);
				}
				b.ContactStates[a] |= ContactStateFlags.IsInContact;
				if ((b.ContactStates[a] & ContactStateFlags.WasInContact) == 0)
				{
					if (b.OnCollision != null && b.OnCollision(b, a))
					{
						b.ContactStates[a] |= ContactStateFlags.IsSuppressed;
						return true;
					}
				}
			}
			return false;
		}
Пример #23
0
        public void processTouchPoints(ReadOnlyTouchPointCollection touches, List<BlobPair> blobPairs, GameTime gameTime)
        {
            lastTouchPosition = touchPosition;
            int tagID = -1;
            int tagValue = -1;
            Boolean manipulatorControl = false;
            
            if (touches.Count == 2 && touches[0].IsFingerRecognized && touches[1].IsFingerRecognized)
            {
                manipulatorControl = true;
                Manipulator2D[] manipulators;
                manipulators = new Manipulator2D[] { 
                    new Manipulator2D(1, touches[1].X, touches[1].Y),
                    new Manipulator2D(3, touches[0].X, touches[0].Y)
                };

                manipulationProcessor.Pivot.X = touches[0].X;
                manipulationProcessor.Pivot.Y = touches[0].Y;

                manipulationProcessor.ProcessManipulators(Timestamp, manipulators);
            }
            else
            {
                manipulationProcessor.CompleteManipulation(Timestamp);
            }

            if (touches.Count >= 1)
            {
                for (int i = 0; i < touches.Count; i++)
                {
                    if (touches[i].IsTagRecognized)
                    {
                        tagID = touches[i].Id;
                        tagValue = (int)touches[i].Tag.Value;
                        break;
                    }
                }

                /*switch (tagValue)
                {
                    case 4:
                        viewManager.rotateToSide(4,t);
                        break;
                    case 5:
                        viewManager.rotateToSide(5,t);
                        break;
                    case 6:
                        viewManager.rotateToSide(6,t);
                        break;
                    case 7:
                        viewManager.rotateToSide(7,t);
                        break;
                    case 8:
                        viewManager.rotateToSide(8,t);
                        break;
                }
                */
                touchPosition = touches[0];
                //First time touch
                if (lastTouchPosition == null)
                {
                    Segment s;
                    s.P1 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.X, touchPosition.Y, 0f),
                        viewManager.Projection, viewManager.View, Matrix.Identity);
                    s.P2 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.X, touchPosition.Y, 1f),
                        viewManager.Projection, viewManager.View, Matrix.Identity);
                    float scalar;
                    Vector3 point;
                    var c = physics.BroadPhase.Intersect(ref s, out scalar, out point);

                    if (c != null && c is BodySkin && !(((SolidThing)((BodySkin)c).Owner).getThingType() == 1) && !(((SolidThing)((BodySkin)c).Owner).getThingType() == 2))
                    {
                        pickedObject = ((BodySkin)c).Owner;
                        orientation = pickedObject.Orientation;
                        pickedDistance = scalar;
                        pickedObject.IsActive = true;
                        pickedObjectOffset = pickedObject.Position - point;
                        pickedObject.IsWeightless = true;
                    }
                    //lastOrientation = touches.Count == 1 ? touches[0].Orientation : touches[1].Orientation;
                    lastOrientation = touches[0].Orientation;
                }
                else if (pickedObject != null)
                {
                    Segment s;
                    s.P1 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.X, touchPosition.Y, 0f),
                        viewManager.Projection, viewManager.View, Matrix.Identity);
                    s.P2 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.X, touchPosition.Y, 1f),
                        viewManager.Projection, viewManager.View, Matrix.Identity);

                    Vector3 diff, point;
                    Vector3.Subtract(ref s.P2, ref s.P1, out diff);
                    Vector3.Multiply(ref diff, pickedDistance, out diff);
                    Vector3.Add(ref s.P1, ref diff, out point);                    
                    pickedObject.SetVelocity(Vector3.Zero, Vector3.Zero);

                    if (!manipulatorControl)
                    {
                        Vector3 position = Vector3.Add(point, pickedObjectOffset);
                        pickedObject.SetWorld(position, orientation);
                    }
                    
                    pickedObject.IsActive = true;
                    SolidThing pickedObjectST = (SolidThing)pickedObject;

                    switch (tagValue)
                    {

                        //Pin a block
                        case 0:
                            pickedObject.Unfreeze();
                            pickedObject.Freeze();
                            break;
                        //unPin a block
                        case 1:
                            if (pickedObjectST.getThingType() == 1)
                            {
                        
                                break;
                            }
                            else
                            {
                                pickedObject.Unfreeze();
                                break;
                            }

                        //Rotate a block
                        case 2:
                            pickedObject.SetWorld(pickedObject.Position, Quaternion.CreateFromAxisAngle(new Vector3(0, 0, -1.0f), touchPosition.Orientation));
                            break;
                        //Move a block towards or away from camera
                        case 3:
                            TouchPoint tagPoint = touches.GetTouchPointFromId(tagID);
                            float deltaRotation = MathHelper.ToDegrees(lastOrientation) - MathHelper.ToDegrees(tagPoint.Orientation);

                            Vector3 direction = new Vector3(0, 0, 1.0f);
                            direction.Normalize();
                            pickedObject.SetWorld(Vector3.Add(pickedObject.Position, Vector3.Multiply(direction, deltaRotation * 0.03f)));
                            lastOrientation = tagPoint.Orientation;
                            break;
                            
                    }




                }
                else if (pickedObject != null)
                {
                    pickedObject.IsWeightless = false;
                    pickedObject = null;                    
                }
            }
            else if (pickedObject != null)
            {
                pickedObject.IsWeightless = false;
                pickedObject = null;
                touchPosition = null;
                lastTouchPosition = null;
                
            }
            else
            {
                touchPosition = null;
            }

        }
Пример #24
0
        /// <summary>
        /// Prepare the contact constraint for iterative processing within a single frame. Computes the desired target velocities
        /// to attempt to prevent inter-penetration.
        /// </summary>
        public override void PreProcess()
        {
            if (this.IsCollisionSuppressed)
            {
                return;
            }

            RigidBody a = BodyA, b = BodyB;
            var       cached = b.Manager.ContactCache.Get(a, b);

            bool isWarmStarted = cached != null && cached.Count == _count;

            // calculate relative force applied during this frame
            Vector3 va = Vector3.Zero, vb = Vector3.Zero;
            float   forceMag;

            if (a.IsMovable)
            {
                Vector3.Multiply(ref a.Force, this.Manager.TimeStep * a.Mass.MassInverse, out va);
            }
            if (b.IsMovable)
            {
                Vector3.Multiply(ref b.Force, this.Manager.TimeStep * b.Mass.MassInverse, out vb);
            }
            Vector3.Add(ref va, ref vb, out va);
            Vector3.Dot(ref _normal, ref va, out forceMag);
            forceMag = MathHelper.Min(forceMag, 0f);

            for (int i = 0; i < _count; i++)
            {
                var p = _points[i];

                // calculate movement along normal and tangent
                float normalDelta;
                a.GetVelocityAtPoint(ref p.OffsetA, out va);
                b.GetVelocityAtPoint(ref p.OffsetB, out vb);
                Vector3.Subtract(ref va, ref vb, out va);
                Vector3.Dot(ref _normal, ref va, out normalDelta);

                float tangentDelta;
                Vector3.Multiply(ref _normal, normalDelta, out p.Tangent);
                Vector3.Subtract(ref va, ref p.Tangent, out p.Tangent);
                Vector3.Dot(ref p.Tangent, ref va, out tangentDelta);
                if (p.Tangent.LengthSquared() >= Constants.Epsilon)
                {
                    p.Tangent.Normalize();
                    Vector3.Negate(ref p.Tangent, out p.Tangent);
                }
                else
                {
                    p.Tangent = Vector3.Zero;
                }

                // calculate effective mass along tangent and normal
                p.NormalMass  = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref _normal);
                p.TangentMass = p.Tangent != Vector3.Zero ?
                                MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref p.Tangent) : 0f;

                // calculate target velocity
                float restitution = Math.Max(_restitution * -(normalDelta - forceMag), 0f);
                float penetration = (p.Depth - this.Manager.LinearErrorTolerance);
                if (restitution < this.Manager.MinRestitution)
                {
                    if (penetration > 0f)
                    {
                        p.Target = penetration * this.Manager.PenetrationBias;
                    }
                    else
                    {
                        float scale = MathHelper.Clamp(-0.1f * penetration / this.Manager.LinearErrorTolerance, Constants.Epsilon, 1f);
                        p.Target = scale * (p.Depth - this.Manager.LinearErrorTolerance) * this.Manager.TimeStepInverse;
                    }
                }
                else
                {
                    p.Target = Math.Max(Math.Max(penetration * this.Manager.PenetrationBias, 0f), restitution);
                }

                p.Impulse         = 0f;
                p.TangentImpulse  = 0f;
                p.PositionImpulse = 0f;

                if (isWarmStarted)
                {
                    Vector3 impulse, tangentImpulse;

                    // find the best cached point
                    var   bestPoint    = new CachedContactPoint();
                    float bestDistance = float.MaxValue;
                    for (int j = 0; j < cached.Points.Length; j++)
                    {
                        float d1, d2;
                        Vector3.DistanceSquared(ref cached.Points[j].OffsetA, ref p.OffsetA, out d1);
                        Vector3.DistanceSquared(ref cached.Points[j].OffsetB, ref p.OffsetB, out d2);
                        if (d1 + d2 < bestDistance)
                        {
                            bestDistance = d1 + d2;
                            bestPoint    = cached.Points[j];
                        }
                    }

                    p.Impulse = bestPoint.NormalImpulse;
                    float tangentImpulseMag = MathHelper.Clamp(-tangentDelta * p.TangentMass, 0f, _friction * p.Impulse);
                    p.TangentImpulse = tangentImpulseMag * p.NormalMass;

                    if (Math.Abs(p.Impulse) >= Constants.Epsilon)
                    {
                        Vector3.Multiply(ref _normal, p.Impulse, out impulse);
                        Vector3.Multiply(ref p.Tangent, p.TangentImpulse, out tangentImpulse);
                        Vector3.Add(ref impulse, ref tangentImpulse, out impulse);
                        a.ApplyImpulse(ref impulse, ref p.OffsetA);
                        Vector3.Negate(ref impulse, out impulse);
                        b.ApplyImpulse(ref impulse, ref p.OffsetB);
                    }
                }
                _points[i] = p;
            }

            // calculate an averaged contact point for help with stabilization during position correction
            if (_count > 2 && _count < _points.Length)
            {
                var ap = _points[_count];
                ap.Depth = float.MaxValue;
                for (int i = 0; i < _count; i++)
                {
                    var p = _points[i];

                    float   depth;
                    Vector3 pa, pb;
                    Vector3.Add(ref a.World.Position, ref p.OffsetA, out pa);
                    Vector3.Add(ref b.World.Position, ref p.OffsetB, out pb);
                    Vector3.Add(ref ap.OffsetA, ref pa, out ap.OffsetA);
                    Vector3.Add(ref ap.OffsetB, ref pb, out ap.OffsetB);
                    Vector3.Subtract(ref pb, ref pa, out pa);
                    Vector3.Dot(ref _normal, ref pa, out depth);
                    if (depth < ap.Depth)
                    {
                        ap.Depth = depth;
                    }
                }
                Vector3.Divide(ref ap.OffsetA, _count, out ap.OffsetA);
                Vector3.Divide(ref ap.OffsetB, _count, out ap.OffsetB);
                ap.NormalMass      = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref ap.OffsetA, ref ap.OffsetB, ref _normal);
                ap.PositionImpulse = 0f;
                _points[_count]    = ap;
            }
        }
Пример #25
0
		/// <summary>
		/// Construct a new joint.
		/// </summary>
		/// <param name="bodyA">The first body to constrain.</param>
		/// <param name="bodyB">The second body to constrain.</param>
		public Joint(RigidBody bodyA, RigidBody bodyB)
			: base(bodyA, bodyB)
		{
			_constraints = new List<Constraint>();
		}
Пример #26
0
		/// <summary>
		/// Construct a new motor force.
		/// </summary>
		/// <param name="body">The body on which to apply torque.</param>
		/// <param name="worldTorque">The axis and magnitude of the torque in world-space.</param>
		public MotorForce(RigidBody body, Vector3 worldTorque)
		{
			_body = body;
			Vector3.Transform(ref worldTorque, ref _body.WorldInverse.Orientation, out _torque);
		}