BEPUphysics

Code: Select all

/*
      Copyright (C) 2011 Bepu Entertainment LLC.

      This software source code 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.

    Contact us at:
    contact@bepu-games.com
 */

// Modified by Rhys Perkins 2011

using Microsoft.Xna.Framework;
using BEPUphysics;
using BEPUphysics.Entities;
using BEPUphysics.MathExtensions;
using BEPUphysics.Constraints;
using BEPUphysics.CollisionRuleManagement;
using BEPUphysics.UpdateableSystems;
using BEPUphysics.Constraints.SingleEntity;
using BEPUphysics.Constraints.TwoEntity.Motors;

namespace Prototype1
{
    /// <summary>
    /// Grabs an entity at a specified location and applies corrective impulses to keep the grabbed location near the goal location
    /// </summary>
    public class MotorizedGrabSpring : Updateable, IEndOfFrameUpdateable
    {
        private SingleEntityAngularMotor angularMotor;
        private Game1 game;    
        private SingleEntityLinearMotor linearMotor;
        private Vector3 localOffsetOrientation;
        private CollisionRule originalEntityCollisonRule;

        /// <summary>
        /// Constructs a grab constraint.
        /// </summary>
        public MotorizedGrabSpring(Game1 game)
        {
            this.game = game;

            // Note that when the motor is created using the empty constructor, it starts deactivated
            // This prevents explosions from attempting to update it without being configured
            angularMotor = new SingleEntityAngularMotor();
            angularMotor.Settings.Mode = MotorMode.Servomechanism;

            linearMotor = new SingleEntityLinearMotor();         
            linearMotor.Settings.Mode = MotorMode.Servomechanism;

            IsUpdating = false;

            game.Space.Add(this);
        }

        /// <summary>
        /// Reinitializes the grabbing constraint with new information.
        /// </summary>
        /// <param name="e">Entity to grab.</param>
        /// <param name="grabLocation">Location on the entity being grabbed in world space.</param>
        public void Setup(Entity e, Vector3 grabLocation, GrabMode grabMode)
        {
            Entity = e;

            // Get the original entity properties that will need to be changed for this GrabSpring to function correctly
            originalEntityCollisonRule = Entity.CollisionInformation.CollisionRules.Personal;

            // Can I assign a rule to this entity's collision pairs?
            // This way the other object in the collision pair will also react in the same way as this object for that particular collision.
            if (!(Entity.CollisionInformation.Tag is EntityAttractor))
            {
                Entity.CollisionInformation.CollisionRules.Personal = CollisionRule.NoBroadPhase;
            }

            LocalOffset = Vector3.Transform(grabLocation - e.Position, Quaternion.Conjugate(e.Orientation));
            localOffsetOrientation = grabLocation - e.Position;

            GrabbedOrientation = e.Orientation;
            GoalPosition = grabLocation;

            angularMotor.Settings.Servo.Goal = e.Orientation;
            angularMotor.Settings.Servo.SpringSettings.StiffnessConstant = 60000f * Entity.Mass;
            angularMotor.Settings.Servo.SpringSettings.DampingConstant = 900f * Entity.Mass;
            angularMotor.Settings.MaximumForce = 10000f * Entity.Mass;
            angularMotor.Settings.VelocityMotor.Softness = .1f / e.Mass;

            // The stiffness, damping, and maximum force could be assigned during setup if the motor
            // needs to behave similarly for entities of varying masses.  
            // When using a fixed configuration, the grabspring will behave weakly when trying to move extremely heavy objects,
            // while staying very responsive for sufficiently light objects.
            linearMotor.Settings.Servo.SpringSettings.StiffnessConstant = 60000f * Entity.Mass;
            linearMotor.Settings.Servo.SpringSettings.DampingConstant = 900f * Entity.Mass;

            // An unlimited motor will gladly push the entity through other objects.
            // Putting a limit on the strength of the motor will prevent it from doing so.
            linearMotor.Settings.MaximumForce = 10000f * Entity.Mass;

            // Don't use the linear motor when rotating the entity
            linearMotor.IsActive = grabMode == GrabMode.Rotation ? false : true;
        }

        /// <summary>
        /// Releases the entity being held by the grab spring.
        /// </summary>
        public void Release()
        {
            // Reset the entity's collision rule back to its original state
            Entity.CollisionInformation.CollisionRules.Personal = originalEntityCollisonRule;

            Entity = null;
        }

        /// <summary>
        /// Updates the grab constraint's grab position after the end of a frame.
        /// </summary>
        /// <param name="dt">Time since last frame in simulation seconds.</param>
        void IEndOfFrameUpdateable.Update(float dt)
        {
            // Since the grabbed position is usually examined graphically, it's good to use the interpolated positions in case the 
            // engine is using internal time stepping and interpolation.
            GrabbedPosition = Matrix3X3.Transform(LocalOffset, Entity.BufferedStates.InterpolatedStates.OrientationMatrix) + Entity.BufferedStates.InterpolatedStates.Position;
        }

        public override void OnAdditionToSpace(ISpace space)
        {
            space.Add(linearMotor);
            space.Add(angularMotor);
        }

        public override void OnRemovalFromSpace(ISpace space)
        {
            space.Remove(linearMotor);
            space.Remove(angularMotor);
        }

        /// <summary>
        /// Gets the grabbed entity.
        /// </summary>
        public Entity Entity
        {
            get { return linearMotor.Entity; }
            private set
            {
                // Don't bother changing the entity if it is the same
                if (linearMotor.Entity != value) 
                {
                    linearMotor.Entity = value;
                    angularMotor.Entity = value;

                    // The motors can only be on while the entity isn't null
                    if (value != null)
                    {
                        linearMotor.IsActive = true;
                        angularMotor.IsActive = true;
                        IsUpdating = true;
                    }
                    else
                    {
                        linearMotor.IsActive = false;
                        angularMotor.IsActive = false;
                        IsUpdating = false;
                    }
                }
            }
        }

        /// <summary>
        /// Gets the location that the entity will be pulled towards.
        /// </summary>
        public Vector3 GoalPosition
        {
            get { return linearMotor.Settings.Servo.Goal; }
            set { linearMotor.Settings.Servo.Goal = value; }
        }

        public Quaternion GoalOrientation
        {
            get { return angularMotor.Settings.Servo.Goal; }
            set { angularMotor.Settings.Servo.Goal = value; }
        }

        /// <summary>
        /// Gets the offset from the entity to the grabbed location in its local space.
        /// </summary>
        public Vector3 LocalOffset
        {
            get { return linearMotor.LocalPoint; }
            private set { linearMotor.LocalPoint = value; }
        }

        public Vector3 LocalOffsetOrientation
        {
            get { return localOffsetOrientation; }
        }

        public Quaternion GrabbedOrientation { get; private set; }

        /// <summary>
        /// Gets the last updated position of the grab location on the surface of the entity.
        /// </summary>
        public Vector3 GrabbedPosition { get; private set; }

        /// <summary>
        /// Gets whether or not the grabber is currently grabbing something.
        /// </summary>
        public bool IsGrabbing
        {
            get { return Entity != null; }
        }
    }
}

Code: Select all

        private void RotateSelectedObjectWithGrabSpring()
        {
            CollisionDetection.Ray cursorRay = cursor.Ray;

            Vector3 currentPosition = cursorRay.Origin + cursorRay.Direction * grabDistance;
            Vector3 entityPosition = grabSpring.Entity.Position;
            Quaternion orientation;

            // Constrain object movement based on selected rotation axis
            switch (rotationWidget.AxisSelected)
            {
                case AxisSelected.All:
                    {
                        orientation = Quaternion.Identity;
                        break;
                    }
                case AxisSelected.None:
                    {
                        orientation = Quaternion.Identity;
                        break;
                    }
                case AxisSelected.X:
                    {
                        //float originalAngle = (float)Math.Atan2(grabSpring.GrabbedPosition.Z - entityPosition.Z, grabSpring.GrabbedPosition.Y - entityPosition.Y);
                        float originalAngle = (float)Math.Atan2(grabSpring.LocalOffsetOrientation.Z, grabSpring.LocalOffsetOrientation.Y);
                        float newAngle = (float)Math.Atan2(currentPosition.Z - entityPosition.Z, currentPosition.Y - entityPosition.Y);
                        orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitX, newAngle - originalAngle);

                        break;
                    }
                case AxisSelected.Y:
                    {
                        //float originalAngle = (float)Math.Atan2(grabSpring.GrabbedPosition.X - entityPosition.X, grabSpring.GrabbedPosition.Z - entityPosition.Z);
                        float originalAngle = (float)Math.Atan2(grabSpring.LocalOffsetOrientation.X, grabSpring.LocalOffsetOrientation.Z);
                        float newAngle = (float)Math.Atan2(currentPosition.X - entityPosition.X, currentPosition.Z - entityPosition.Z);
                        orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitY, newAngle - originalAngle);

                        break;
                    }
                case AxisSelected.Z:
                    {
                        //float originalAngle = (float)Math.Atan2(grabSpring.GrabbedPosition.Y - entityPosition.Y, grabSpring.GrabbedPosition.X - entityPosition.X);
                        float originalAngle = (float)Math.Atan2(grabSpring.LocalOffsetOrientation.Y, grabSpring.LocalOffsetOrientation.X);
                        float newAngle = (float)Math.Atan2(currentPosition.Y - entityPosition.Y, currentPosition.X - entityPosition.X);
                        orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, newAngle - originalAngle);

                        break;
                    }
                default:
                    {
                        orientation = Quaternion.Identity;
                        break;
                    }
            }

            grabSpring.GoalOrientation = Quaternion.Concatenate(grabSpring.GrabbedOrientation, orientation);
        }

Code: Select all

LocalOffset = grabMode == GrabMode.Rotation ? Vector3.Zero : Vector3.Transform(grabLocation - e.Position, Quaternion.Conjugate(e.Orientation));
GoalPosition = grabMode == GrabMode.Rotation ? e.Position : grabLocation;