Superclass of movable rigid bodies. Contains information for both dynamic and kinematic simulation.
Inheritance: IBroadPhaseEntryOwner, IDeferredEventCreatorOwner, ISimulationIslandMemberOwner, ICCDPositionUpdateable, IForceUpdateable, ISpaceObject, IMaterialOwner, ICollisionRulesOwner
 public FixedOffsetCameraControlScheme(Entity entity, Camera camera, DemosGame game)
     : base(camera, game)
 {
     Entity = entity;
     UseCameraSmoothing = true;
     CameraOffset = new Vector3(0, 0.7f, 0);
 }
Exemple #2
0
 /// <summary>
 /// Constructs a thruster originating at the given position, pushing in the given direction.
 /// </summary>
 /// <param name="targetEntity">Entity that the force will be applied to.</param>
 /// <param name="pos">Origin of the force.</param>
 /// <param name="dir">Direction of the force.</param>
 /// <param name="time">Total lifespan of the force.  A lifespan of zero is infinite.</param>
 public Thruster(Entity targetEntity, Vector3 pos, Vector3 dir, float time)
 {
     Target = targetEntity;
     Position = pos;
     Direction = dir;
     LifeSpan = time;
 }
        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public FishInABarrelDemo(DemosGame game)
            : base(game)
        {
            game.Camera.Position = new Vector3(0, 7, 30);

            var detector = new Box(new Vector3(0, 0, 0), 1.5f, 1.5f, 1.5f);
            detector.CollisionInformation.CollisionRules.Personal = CollisionRule.NoSolver;
            var acceptedTriggerEntity = new Box(new Vector3(5, 0, 0), 1.6f, .7f, .4f, 1);
            acceptedTrigger = acceptedTriggerEntity.CollisionInformation;

            detector.Tag = "noDisplayObject";
            acceptedTriggerEntity.Tag = "noDisplayObject";
            Space.Add(detector);
            Space.Add(acceptedTriggerEntity);

            var fish = game.Content.Load<Model>("fish");
            game.ModelDrawer.Add(new DisplayEntityModel(acceptedTriggerEntity, fish, game.ModelDrawer));

            var barrelAndPlatform = game.Content.Load<Model>("barrelAndPlatform");
            Vector3[] staticTriangleVertices;
            int[] staticTriangleIndices;
            ModelDataExtractor.GetVerticesAndIndicesFromModel(barrelAndPlatform, out staticTriangleVertices, out staticTriangleIndices);

            //Note that the final 'margin' parameter is optional, but can be used to specify a collision margin on triangles in the static triangle group.
            var fishDepositoryGroup = new StaticMesh(staticTriangleVertices, staticTriangleIndices);
            CollisionRules.AddRule(fishDepositoryGroup, detector, CollisionRule.NoBroadPhase);
            Space.Add(fishDepositoryGroup);
            game.ModelDrawer.Add(fishDepositoryGroup);

            movedBox = new Box(new Vector3(-4, 5, 0), 1, 1, 1, 1);
            detector.Space.Add(movedBox);
            detector.CollisionInformation.Events.InitialCollisionDetected += InitialCollisionDetected;
            detector.CollisionInformation.Events.CollisionEnded += CollisionEnded;
        }
 /// <summary>
 /// Constructs a new display model.
 /// </summary>
 /// <param name="entity">Entity to follow.</param>
 /// <param name="model">Model to draw on the entity.</param>
 /// <param name="modelDrawer">Model drawer to use.</param>
 public DisplayEntityModel(Entity entity, Model model, ModelDrawer modelDrawer)
     : base(modelDrawer)
 {
     OffsetTransform = Matrix.Identity;
     Entity = entity;
     Model = model;
 }
Exemple #5
0
        /// <summary>
        /// Maintains the position of the character's body above the ground.
        /// </summary>
        /// <param name="supportLocationVelocity">Velocity of the support point connected to the supportEntity.</param>
        /// <param name="supportNormal">The normal at the surface where the ray hit the entity.</param>
        /// <param name="supportDistance">Distance from the character to the support location.</param>
        private void support(Vector3 supportLocationVelocity, Vector3 supportNormal, float supportDistance, float dt)
        {
            //Put the character at the right distance from the ground.
            float supportVerticalVelocity = Math.Max(supportLocationVelocity.Y, -0.1f);
            float heightDifference        = this.SupportHeight - supportDistance;

            this.Body.Position += (new Vector3(0, MathHelper.Clamp(heightDifference, (supportVerticalVelocity - 10.0f) * dt, (supportVerticalVelocity + 10.0f) * dt), 0));

            //Remove from the character velocity which would push it toward or away from the surface.
            //This is a relative velocity, so the velocity of the body and the velocity of a point on the support entity must be found.
            float   bodyNormalVelocity          = Vector3.Dot(this.Body.LinearVelocity, supportNormal);
            float   supportEntityNormalVelocity = Vector3.Dot(supportLocationVelocity, supportNormal);
            Vector3 diff = (bodyNormalVelocity - supportEntityNormalVelocity) * -supportNormal;

            diff.Y = Math.Max(diff.Y, 0);
            this.Body.LinearVelocity += diff;

            BEPUphysics.Entities.Entity supportEntity = this.SupportEntity;
            if (supportEntity != null && supportEntity.IsAffectedByGravity)
            {
                Vector3 supportLocation = this.SupportLocation;
                Vector3 impulse         = (this.Body.Mass * 1.5f) * ((Space)this.Space).ForceUpdater.Gravity * dt;
                supportEntity.ApplyImpulse(ref supportLocation, ref impulse);
                supportEntity.ActivityInformation.Activate();
            }
        }
        /// <summary>
        /// Locates the closest support entity by performing a raycast at collected candidates.
        /// </summary>
        /// <param name="supportEntity">The closest supporting entity.</param>
        /// <param name="supportLocation">The support location where the ray hit the entity.</param>
        /// <param name="supportNormal">The normal at the surface where the ray hit the entity.</param>
        /// <param name="supportDistance">Distance from the character to the support location.</param>
        /// <returns>Whether or not a support was located.</returns>
        private bool FindSupport(out BEPUphysics.Entities.Entity supportEntity, out Vector3 supportLocation, out Vector3 supportNormal, out float supportDistance)
        {
            supportEntity   = null;
            supportLocation = Toolbox.NoVector;
            supportNormal   = Toolbox.NoVector;
            supportDistance = float.MaxValue;

            Vector3 rayOrigin = Body.Position + rayOriginOffset;

            for (int i = 0; i < collisionPairCollector.CollisionInformation.Pairs.Count; i++)
            {
                var pair = collisionPairCollector.CollisionInformation.Pairs[i];
                //Determine which member of the collision pair is the possible support.
                Collidable candidate = (pair.BroadPhaseOverlap.EntryA == collisionPairCollector.CollisionInformation ? pair.BroadPhaseOverlap.EntryB : pair.BroadPhaseOverlap.EntryA) as Collidable;
                //Ensure that the candidate is a valid supporting entity.
                if (candidate.CollisionRules.Personal >= CollisionRule.NoSolver)
                {
                    continue; //It is invalid!
                }
                //The maximum length is supportHeight * 2 instead of supportHeight alone because the character should be able to step downwards.
                //This acts like a sort of 'glue' to help the character stick on the ground in general.
                float maximumDistance;
                //The 'glue' effect should only occur if the character has a solid hold on the ground though.
                //Otherwise, the character is falling or sliding around uncontrollably.
                if (HasTraction)
                {
                    maximumDistance = supportHeight * 2;
                }
                else
                {
                    maximumDistance = supportHeight;
                }

                RayHit rayHit;
                //Fire a ray at the candidate and determine some details!
                if (candidate.RayCast(new Ray(rayOrigin, Vector3.Down), maximumDistance, out rayHit))
                {
                    //We want to find the closest support, so compare it against the last closest support.
                    if (rayHit.T < supportDistance)
                    {
                        supportDistance = rayHit.T;
                        supportLocation = rayHit.Location;
                        supportNormal   = rayHit.T > 0 ? rayHit.Normal : Vector3.Up;

                        var entityInfo = candidate as EntityCollidable;
                        if (entityInfo != null)
                        {
                            supportEntity = entityInfo.Entity;
                        }
                        else
                        {
                            supportEntity = null;
                        }
                    }
                }
            }
            supportNormal.Normalize();
            return(supportDistance < float.MaxValue);
        }
Exemple #7
0
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate.</param>
        /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA.  Will be kept perpendicular with the twist axis.</param>
        public SwivelHingeJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor, ref Vector3 hingeAxis)
        {
            if (connectionA == null)
                connectionA = TwoEntityConstraint.WorldEntity;
            if (connectionB == null)
                connectionB = TwoEntityConstraint.WorldEntity;
            BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor);
			Vector3 tmp;
			BallSocketJoint.OffsetB.Invert( out tmp );
            AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, ref hingeAxis, ref tmp );
            HingeLimit = new RevoluteLimit(connectionA, connectionB);
            HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
            TwistLimit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0);
            TwistMotor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp );
            HingeLimit.IsActive = false;
            HingeMotor.IsActive = false;
            TwistLimit.IsActive = false;
            TwistMotor.IsActive = false;

			//Ensure that the base and test direction is perpendicular to the free axis.
			Vector3 baseAxis; anchor.Sub( ref connectionA.position, out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
                connectionB.position.Sub( ref anchor, out baseAxis );
            baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot( ref baseAxis, ref hingeAxis) , out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
                }
            }
            HingeLimit.Basis.SetWorldAxes(ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);
            HingeMotor.Basis.SetWorldAxes( ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);

			connectionB.position.Sub( ref anchor, out baseAxis );
            baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot(ref baseAxis, ref hingeAxis), out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
                }
            }
            HingeLimit.TestAxis = baseAxis;
            HingeMotor.TestAxis = baseAxis;


            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(HingeLimit);
            Add(HingeMotor);
            Add(TwistLimit);
            Add(TwistMotor);
        }
Exemple #8
0
        /// <summary>
        /// Constructs a new point on plane constraint.
        /// </summary>
        /// <param name="connectionA">Entity to which the constraint's plane is attached.</param>
        /// <param name="connectionB">Entity to which the constraint's point is attached.</param>
        /// <param name="planeAnchor">A point on the plane.</param>
        /// <param name="normal">Direction, attached to the first connected entity, defining the plane's normal</param>
        /// <param name="pointAnchor">The point to constrain to the plane, attached to the second connected object.</param>
        public PointOnPlaneJoint(Entity connectionA, Entity connectionB, Vector3 planeAnchor, Vector3 normal, Vector3 pointAnchor)
        {
            ConnectionA = connectionA;
            ConnectionB = connectionB;

            PointAnchor = pointAnchor;
            PlaneAnchor = planeAnchor;
            PlaneNormal = normal;
        }
Exemple #9
0
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate.</param>
        /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA.  Will be kept perpendicular with the twist axis.</param>
        public SwivelHingeJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 hingeAxis)
        {
            if (connectionA == null)
                connectionA = TwoEntityConstraint.WorldEntity;
            if (connectionB == null)
                connectionB = TwoEntityConstraint.WorldEntity;
            BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
            AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, hingeAxis, -BallSocketJoint.OffsetB);
            HingeLimit = new RevoluteLimit(connectionA, connectionB);
            HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
            TwistLimit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
            TwistMotor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
            HingeLimit.IsActive = false;
            HingeMotor.IsActive = false;
            TwistLimit.IsActive = false;
            TwistMotor.IsActive = false;

            //Ensure that the base and test direction is perpendicular to the free axis.
            Vector3 baseAxis = anchor - connectionA.position;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
                baseAxis = connectionB.position - anchor;
            baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
                }
            }
            HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
            HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);

            baseAxis = connectionB.position - anchor;
            baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
                }
            }
            HingeLimit.TestAxis = baseAxis;
            HingeMotor.TestAxis = baseAxis;


            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(HingeLimit);
            Add(HingeMotor);
            Add(TwistLimit);
            Add(TwistMotor);
        }
Exemple #10
0
 public void refreshModel(GLContext context)
 {
     if (model != pmodel)
     {
         pmodel        = model;
         internalModel = context.Models.GetModel(model);
         if (mode == ModelCollisionMode.PRECISE)
         {
             mesh   = context.Models.Handler.MeshToBepu(internalModel.OriginalModel);
             offset = -Location.FromBVector(mesh.Position);
         }
         else if (mode == ModelCollisionMode.AABB)
         {
             List <BEPUutilities.Vector3> vecs = context.Models.Handler.GetCollisionVertices(internalModel.OriginalModel);
             Location zero = new Location(vecs[0].X, vecs[0].Y, vecs[0].Z);
             AABB     abox = new AABB()
             {
                 Min = zero, Max = zero
             };
             for (int v = 1; v < vecs.Count; v++)
             {
                 abox.Include(new Location(vecs[v].X, vecs[v].Y, vecs[v].Z));
             }
             Location size = abox.Max - abox.Min;
             offset = abox.Max - size / 2;
             mesh   = new BEPUphysics.Entities.Prefabs.Box(
                 new BEPUphysics.EntityStateManagement.MotionState()
             {
                 Position = Position.ToBVector(), Orientation = Angle
             },
                 (float)size.X, (float)size.Y, (float)size.Z);
         }
         else
         {
             List <BEPUutilities.Vector3> vecs = context.Models.Handler.GetCollisionVertices(internalModel.OriginalModel);
             double distSq = 0;
             for (int v = 1; v < vecs.Count; v++)
             {
                 if (vecs[v].LengthSquared() > distSq)
                 {
                     distSq = vecs[v].LengthSquared();
                 }
             }
             double size = Math.Sqrt(distSq);
             offset = Location.Zero;
             mesh   = new BEPUphysics.Entities.Prefabs.Sphere(
                 new BEPUphysics.EntityStateManagement.MotionState()
             {
                 Position = Position.ToBVector(), Orientation = Angle
             }, (float)size);
         }
         offsetmat = Matrix4.CreateTranslation(offset.ToOVector());
     }
     mesh.Position    = Position.ToBVector();
     mesh.Orientation = Angle;
 }
        /// <summary>
        /// [Utility] Recovers the object from entity.
        /// </summary>
        /// <param name="entity">The entity.</param>
        /// <returns></returns>
        public static IObject RecoverObjectFromEntity(BEPUphysics.Entities.Entity entity)
        {
            IPhysicObject phy = (entity.CollisionInformation.Tag as IPhysicObject);

            if (phy != null)
            {
                return(phy.ObjectOwner);
            }
            return(null);
        }
Exemple #12
0
 private static Vector3 GetAnchorGuess(Entity connectionA, Entity connectionB)
 {
     var anchor = new Vector3();
     if (connectionA != null)
         anchor += connectionA.position;
     if (connectionB != null)
         anchor += connectionB.position;
     if (connectionA != null && connectionB != null)
         anchor *= 0.5f;
     return anchor;
 }
Exemple #13
0
 /// <summary>
 /// Constructs a new constraint which restricts the linear and angular motion between two entities.
 /// </summary>
 /// <param name="connectionA">First entity of the constraint pair.</param>
 /// <param name="connectionB">Second entity of the constraint pair.</param>
 /// <param name="anchor">The location of the weld.</param>
 public WeldJoint(Entity connectionA, Entity connectionB, Vector3 anchor)
 {
     if (connectionA == null)
         connectionA = TwoEntityConstraint.WorldEntity;
     if (connectionB == null)
         connectionB = TwoEntityConstraint.WorldEntity;
     BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
     NoRotationJoint = new NoRotationJoint(connectionA, connectionB);
     Add(BallSocketJoint);
     Add(NoRotationJoint);
 }
Exemple #14
0
 /// <summary>
 /// Constructs a new constraint which restricts the linear and angular motion between two entities.
 /// </summary>
 /// <param name="connectionA">First entity of the constraint pair.</param>
 /// <param name="connectionB">Second entity of the constraint pair.</param>
 public WeldJoint(Entity connectionA, Entity connectionB)
 {
     if (connectionA == null)
         connectionA = TwoEntityConstraint.WorldEntity;
     if (connectionB == null)
         connectionB = TwoEntityConstraint.WorldEntity;
     BallSocketJoint = new BallSocketJoint(connectionA, connectionB, (connectionA.position + connectionB.position) * .5f);
     NoRotationJoint = new NoRotationJoint(connectionA, connectionB);
     Add(BallSocketJoint);
     Add(NoRotationJoint);
 }
Exemple #15
0
 private static Vector3 GetAnchorGuess(Entity connectionA, Entity connectionB)
 {
     var anchor = new Vector3();
     if (connectionA != null)
         anchor.Add( ref connectionA.position, out anchor );
     if (connectionB != null)
         anchor.Add( ref connectionB.position, out anchor );
     if (connectionA != null && connectionB != null)
         anchor.Mult( 0.5f, out anchor );
     return anchor;
 }
Exemple #16
0
		/// <summary>
		/// Constructs a new constraint which restricts the linear and angular motion between two entities.
		/// Uses the average of the two entity positions for the anchor.
		/// </summary>
		/// <param name="connectionA">First entity of the constraint pair.</param>
		/// <param name="connectionB">Second entity of the constraint pair.</param>
		public WeldJoint(Entity connectionA, Entity connectionB)
        {
			if( connectionA == null )
				connectionA = TwoEntityConstraint.WorldEntity;
			if( connectionB == null )
				connectionB = TwoEntityConstraint.WorldEntity;
			Vector3 anchor;  GetAnchorGuess( connectionA, connectionB, out anchor );
            BallSocketJoint = new BallSocketJoint( connectionA, connectionB, ref anchor );
			NoRotationJoint = new NoRotationJoint( connectionA, connectionB );
			Add( BallSocketJoint );
			Add( NoRotationJoint );
		}
Exemple #17
0
		private static void GetAnchorGuess( Entity connectionA, Entity connectionB, out Vector3 anchor )
		{
			if( connectionA != null && connectionB != null )
			{
				connectionA.position.Add( ref connectionA.position, out anchor );
				anchor.Mult( 0.5f, out anchor );
			}
			else if( connectionA != null )
				anchor = connectionA.position;
			else if( connectionB != null )
				anchor = connectionB.position;
			anchor = Vector3.Zero;
		}
Exemple #18
0
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate.</param>
        /// <param name="freeAxis">Axis around which the hinge can rotate.</param>
        public RevoluteJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 anchor, System.Numerics.Vector3 freeAxis)
        {
            if (connectionA == null)
                connectionA = TwoEntityConstraint.WorldEntity;
            if (connectionB == null)
                connectionB = TwoEntityConstraint.WorldEntity;
            BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
            AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis);
            Limit = new RevoluteLimit(connectionA, connectionB);
            Motor = new RevoluteMotor(connectionA, connectionB, freeAxis);
            Limit.IsActive = false;
            Motor.IsActive = false;

            //Ensure that the base and test direction is perpendicular to the free axis.
            System.Numerics.Vector3 baseAxis = anchor - connectionA.position;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
                baseAxis = connectionB.position - anchor;
            baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
                }
            }
            Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
            Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);

            baseAxis = connectionB.position - anchor;
            baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
                }
            }
            Limit.TestAxis = baseAxis;
            Motor.TestAxis = baseAxis;

            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(Limit);
            Add(Motor);
        }
Exemple #19
0
        /// <summary>
        /// Creates a new EntityModel.
        /// </summary>
        /// <param name="entity">Entity to attach the graphical representation to.</param>
        /// <param name="model">Graphical representation to use for the entity.</param>
        /// <param name="transform">Base transformation to apply to the model before moving to the entity.</param>
        /// <param name="game">Game to which this component will belong.</param>
        public EntityModel(Entity entity, Model model, Matrix transform, Game game, Player player)
            : base(game)
        {
            this.entity = entity;
            this.model = model;
            Transform = transform;
            Player = player;

            //Collect any bone transformations in the model itself.
            //The default cube model doesn't have any, but this allows the EntityModel to work with more complicated shapes.
            boneTransforms = new Matrix[model.Bones.Count];
            foreach (var effect in model.Meshes.SelectMany(mesh => mesh.Effects).Cast<BasicEffect>())
            {
                effect.EnableDefaultLighting();
            }
        }
 /// <summary>
 /// Calculates the impulse to apply to the center of mass of physically simulated bodies within the field.
 /// </summary>
 /// <param name="e">Target of the impulse.</param>
 /// <param name="dt">Time since the last frame in simulation seconds.</param>
 /// <param name="impulse">Force to apply at the given position.</param>
 protected override void CalculateImpulse(Entity e, float dt, out Vector3 impulse)
 {
     if (MaximumPushSpeed > 0)
     {
         //Current velocity along the tangent direction.
         float dot = Vector3.Dot(e.LinearVelocity, forceDirection);
         //Compute the velocity difference between the current and the maximum
         dot = MaximumPushSpeed - dot;
         //Compute the force needed to reach the maximum, but clamp it to the amount of force that the field can apply
         //Also, don't apply a force that would slow an object down.
         dot = MathHelper.Clamp(dot * e.Mass, 0, forceMagnitude * dt);
         Vector3.Multiply(ref forceDirection, dot, out impulse);
     }
     else
         impulse = Force * dt;
 }
        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public DetectorVolumeDemo(DemosGame game)
            : base(game)
        {
            var model = game.Content.Load<Model>("tube");
            Vector3[] modelVertices;
            int[] modelIndices;
            ModelDataExtractor.GetVerticesAndIndicesFromModel(model, out modelVertices, out modelIndices);
            detectorVolume = new DetectorVolume(new TriangleMesh(new StaticMeshData(modelVertices, modelIndices)));
            Space.Add(detectorVolume);

            game.ModelDrawer.Add(detectorVolume.TriangleMesh);

            //The detector volume works on all of the entity types:
            //Convexes!
            testEntity = new Box(new Vector3(0, -10, 0), 1, 1, 1);

            //Compounds!
            //var bodies = new List<CompoundShapeEntry>
            //{
            //    new CompoundShapeEntry(new SphereShape(.5f), new Vector3(0, -8.2f, 0), 1),
            //    new CompoundShapeEntry(new SphereShape(.5f), new Vector3(0, -9f, 0), 1),
            //    new CompoundShapeEntry(new SphereShape(.5f), new Vector3(0, -9.8f, 0), 1)
            //};
            //testEntity = new CompoundBody(bodies);

            //Mobile meshes!
            //model = game.Content.Load<Model>("tube");
            //TriangleMesh.GetVerticesAndIndicesFromModel(model, out modelVertices, out modelIndices);
            //testEntity = new MobileMesh(modelVertices, modelIndices, new AffineTransform(new Vector3(.2f, .2f, .2f), Quaternion.Identity, new Vector3(0, -10, 0)), MobileMeshSolidity.Solid);

            testEntity.Tag = "noDisplayObject";
            displayBox = game.ModelDrawer.Add(testEntity);
            SetColor(0);
            game.ModelDrawer.IsWireframe = true;
            testEntity.LinearVelocity = new Vector3(0, 1, 0);
            Space.Add(testEntity);

            //The color of the entity will change based upon events.
            //The events don't pay attention to the causing events, so you can toss a ball through the volume to recolor the entity.
            detectorVolume.EntityBeganTouching += BeganTouching;
            detectorVolume.EntityStoppedTouching += StoppedTouching;
            detectorVolume.VolumeBeganContainingEntity += BeganContaining;
            detectorVolume.VolumeStoppedContainingEntity += StoppedContaining;

            game.Camera.Position = new Vector3(0, 0, 22);
            Space.ForceUpdater.Gravity = new Vector3();
        }
 /// <summary>
 /// Constructs a new constraint which restricts two degrees of linear freedom and two degrees of angular freedom between two entities.
 /// </summary>
 /// <param name="connectionA">First entity of the constraint pair.</param>
 /// <param name="connectionB">Second entity of the constraint pair.</param>
 /// <param name="lineAnchor">Location of the anchor for the line to be attached to connectionA in world space.</param>
 /// <param name="lineDirection">Axis in world space to be attached to connectionA along which connectionB can move and rotate.</param>
 /// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
 public LineSliderJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 lineAnchor, System.Numerics.Vector3 lineDirection, System.Numerics.Vector3 pointAnchor)
 {
     if (connectionA == null)
         connectionA = TwoEntityConstraint.WorldEntity;
     if (connectionB == null)
         connectionB = TwoEntityConstraint.WorldEntity;
     PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor);
     AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, lineDirection);
     Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0);
     Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection);
     Limit.IsActive = false;
     Motor.IsActive = false;
     Add(PointOnLineJoint);
     Add(AngularJoint);
     Add(Limit);
     Add(Motor);
 }
 /// <summary>
 /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities.
 /// </summary>
 /// <param name="connectionA">First entity of the constraint pair.</param>
 /// <param name="connectionB">Second entity of the constraint pair.</param>
 /// <param name="anchor">Point around which both entities rotate in world space.</param>
 public UniversalJoint(Entity connectionA, Entity connectionB, Vector3 anchor)
 {
     if (connectionA == null)
         connectionA = TwoEntityConstraint.WorldEntity;
     if (connectionB == null)
         connectionB = TwoEntityConstraint.WorldEntity;
     BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
     TwistJoint = new TwistJoint(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
     Limit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
     Motor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
     Limit.IsActive = false;
     Motor.IsActive = false;
     Add(BallSocketJoint);
     Add(TwistJoint);
     Add(Limit);
     Add(Motor);
 }
 /// <summary>
 /// Constructs a new constraint which restricts two degrees of linear freedom and all three degrees of angular freedom.
 /// </summary>
 /// <param name="connectionA">First entity of the constraint pair.</param>
 /// <param name="connectionB">Second entity of the constraint pair.</param>
 /// <param name="lineAnchor">Location of the anchor for the line to be attached to connectionA in world space.</param>
 /// <param name="lineDirection">Axis in world space to be attached to connectionA along which connectionB can move.</param>
 /// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
 public PrismaticJoint(Entity connectionA, Entity connectionB, Vector3 lineAnchor, Vector3 lineDirection, Vector3 pointAnchor)
 {
     if (connectionA == null)
         connectionA = TwoEntityConstraint.WorldEntity;
     if (connectionB == null)
         connectionB = TwoEntityConstraint.WorldEntity;
     PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor);
     AngularJoint = new NoRotationJoint(connectionA, connectionB);
     Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0);
     Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection);
     Limit.IsActive = false;
     Motor.IsActive = false;
     Add(PointOnLineJoint);
     Add(AngularJoint);
     Add(Limit);
     Add(Motor);
 }
        /// <summary>
        /// Creates a new EntityModel.
        /// </summary>
        /// <param name="entity">Entity to attach the graphical representation to.</param>
        /// <param name="model">Graphical representation to use for the entity.</param>
        /// <param name="transform">Base transformation to apply to the model before moving to the entity.</param>
        /// <param name="game">Game to which this component will belong.</param>
        public EntityModel(Entity entity, Model model, Matrix transform, Game game, Effect effect)
            : base(game)
        {
            this.entity = entity;
            this.model = model;
            this.Transform = transform;

            //Go through each mesh and assign pre-made effects
            for (int i = 0; i < model.Meshes.Count; i++)
            {
                this.effect.Add(model.Meshes[i].Effects[0]);
            }

            //Collect any bone transformations in the model itself.
            //The default cube model doesn't have any, but this allows the EntityModel to work with more complicated shapes.
            boneTransforms = new Matrix[model.Bones.Count];
        }
Exemple #26
0
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate in world space.</param>
        public UniversalJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor)
        {
            if (connectionA == null)
                connectionA = TwoEntityConstraint.WorldEntity;
            if (connectionB == null)
                connectionB = TwoEntityConstraint.WorldEntity;
            BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor);
			Vector3 tmp; BallSocketJoint.OffsetB.Invert( out tmp );
            TwistJoint = new TwistJoint(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp);
            Limit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0);
            Motor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp );
            Limit.IsActive = false;
            Motor.IsActive = false;
            Add(BallSocketJoint);
            Add(TwistJoint);
            Add(Limit);
            Add(Motor);
        }
Exemple #27
0
        /// <summary>
        /// Creates a new EntityModel.
        /// </summary>
        /// <param name="entity">Entity to attach the graphical representation to.</param>
        /// <param name="model">Graphical representation to use for the entity.</param>
        /// <param name="transform">Base transformation to apply to the model before moving to the entity.</param>
        /// <param name="game">Game to which this component will belong.</param>
        public EntityModel(Entity entity, Model model, Matrix transform, Game game)
            : base(game)
        {
            this.entity = entity;
            this.model = model;
            this.Transform = transform;

            //Collect any bone transformations in the model itself.
            //The default cube model doesn't have any, but this allows the EntityModel to work with more complicated shapes.
            boneTransforms = new Matrix[model.Bones.Count];
            foreach (ModelMesh mesh in model.Meshes)
            {
                foreach (BasicEffect effect in mesh.Effects)
                {
                    effect.EnableDefaultLighting();
                }
            }
        }
Exemple #28
0
        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public MPRCastingDemo(DemosGame game)
            : base(game)
        {
            bShape = new BoxShape(1, 0, 1);
            //bShape.CollisionMargin = 0;
            aShape = new ConeShape(1, .4f);
            //aShape.CollisionMargin = 0;
            a = new Entity(aShape);
            b = new Entity(bShape);
            CollisionRules.AddRule(a, b, CollisionRule.NoSolver);
            NarrowPhaseHelper.CollisionManagers.Remove(new TypePair(typeof(ConvexCollidable<BoxShape>), typeof(ConvexCollidable<BoxShape>)));
            Space.Add(a);
            Space.Add(b);
            a.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver4);
            b.Orientation = Quaternion.Identity;
            aTransform = new RigidTransform(new Vector3(-10, -10, -10), a.Orientation);
            bTransform = new RigidTransform(new Vector3(10, 10, 10), b.Orientation);

            game.Camera.Position = new Vector3(0, 5, 17);
        }
Exemple #29
0
 /// <summary>
 /// Constructs a new constraint which restricts one linear degree of freedom between two entities.
 /// </summary>
 /// <param name="connectionA">First entity of the constraint pair.</param>
 /// <param name="connectionB">Second entity of the constraint pair.</param>
 /// <param name="planeAnchor">Location of the anchor for the plane to be attached to connectionA in world space.</param>
 /// <param name="planeNormal">Normal of the plane constraint in world space.</param>
 /// <param name="xAxis">Direction in world space along which the X axis LinearAxisLimit and LinearAxisMotor work.
 /// This is usually chosen to be perpendicular to the planeNormal and the yAxis.</param>
 /// <param name="yAxis">Direction in world space along which the Y axis LinearAxisLimit and LinearAxisMotor work.
 /// This is usually chosen to be perpendicular to the planeNormal and the xAxis.</param>
 /// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
 public PlaneSliderJoint(Entity connectionA, Entity connectionB, Vector3 planeAnchor, Vector3 planeNormal, Vector3 xAxis, Vector3 yAxis, Vector3 pointAnchor)
 {
     if (connectionA == null)
         connectionA = TwoEntityConstraint.WorldEntity;
     if (connectionB == null)
         connectionB = TwoEntityConstraint.WorldEntity;
     PointOnPlaneJoint = new PointOnPlaneJoint(connectionA, connectionB, planeAnchor, planeNormal, pointAnchor);
     LimitX = new LinearAxisLimit(connectionA, connectionB, planeAnchor, pointAnchor, xAxis, 0, 0);
     MotorX = new LinearAxisMotor(connectionA, connectionB, planeAnchor, pointAnchor, xAxis);
     LimitY = new LinearAxisLimit(connectionA, connectionB, planeAnchor, pointAnchor, yAxis, 0, 0);
     MotorY = new LinearAxisMotor(connectionA, connectionB, planeAnchor, pointAnchor, yAxis);
     LimitX.IsActive = false;
     MotorX.IsActive = false;
     LimitY.IsActive = false;
     MotorY.IsActive = false;
     Add(PointOnPlaneJoint);
     Add(LimitX);
     Add(MotorX);
     Add(LimitY);
     Add(MotorY);
 }
        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public SelfCollidingClothDemo(DemosGame game)
            : base(game)
        {
            //Joints can also act like springs by modifying their springSettings.
            //Though using a bunch of DistanceJoint objects can be slower than just applying direct spring forces,
            //it is significantly more stable and allows rigid structures.
            //The extra stability can make it useful for cloth-like simulations.
            Entity latticePiece;
            BallSocketJoint joint;

            NarrowPhaseHelper.Factories.BoxBox.Count = 4000;
            NarrowPhaseHelper.Factories.BoxSphere.Count = 1000;

            int numColumns = 40;
            int numRows = 40;
            float xSpacing = 1.0f;
            float zSpacing = 1.0f;
            var lattice = new Entity[numRows, numColumns];
            for (int i = 0; i < numRows; i++)
                for (int j = 0; j < numColumns; j++)
                {
                    latticePiece = new Box(
                        new Vector3(
                            xSpacing * i - (numRows - 1) * xSpacing / 2f,
                            15.58f,
                            2 + zSpacing * j - (numColumns - 1) * zSpacing / 2f),
                        xSpacing, .2f, zSpacing, 10);

                    lattice[i, j] = latticePiece;

                    Space.Add(latticePiece);
                }
            //The joints composing the cloth can have their max iterations set independently from the solver iterations.
            //More iterations (up to the solver's own max) will increase the quality at the cost of speed.
            int clothIterations = 3;
            //So while the above clamps joint iterations, setting the solver's iteration limit can lower the
            //rest of the solving load (collisions).
            Space.Solver.IterationLimit = 10;

            float damping = 20000, stiffness = 20000;
            float starchDamping = 5000, starchStiffness = 500;

            //Loop through the grid and set up the joints.
            for (int i = 0; i < numRows; i++)
                for (int j = 0; j < numColumns; j++)
                {
                    if (i == 0 && j + 1 < numColumns)
                    {
                        //Add in column connections for left edge.
                        joint = new BallSocketJoint(lattice[0, j], lattice[0, j + 1], lattice[0, j].Position + new Vector3(-xSpacing / 2, 0, zSpacing / 2));
                        joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);
                    }
                    if (i == numRows - 1 && j + 1 < numColumns)
                    {
                        //Add in column connections for right edge.
                        joint = new BallSocketJoint(lattice[numRows - 1, j], lattice[numRows - 1, j + 1], lattice[numRows - 1, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
                        joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);
                    }
                    if (i + 1 < numRows && j == 0)
                    {
                        //Add in row connections for top edge.
                        joint = new BallSocketJoint(lattice[i, 0], lattice[i + 1, 0], lattice[i, 0].Position + new Vector3(xSpacing / 2, 0, -zSpacing / 2));
                        joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);
                    }
                    if (i + 1 < numRows && j == numColumns - 1)
                    {
                        //Add in row connections for bottom edge.
                        joint = new BallSocketJoint(lattice[i, numColumns - 1], lattice[i + 1, numColumns - 1], lattice[i, numColumns - 1].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
                        joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);

                    }

                    if (i + 1 < numRows && j + 1 < numColumns)
                    {
                        //Add in interior connections.
                        joint = new BallSocketJoint(lattice[i, j], lattice[i + 1, j], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
                        joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);

                        joint = new BallSocketJoint(lattice[i, j], lattice[i, j + 1], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
                        joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);

                        joint = new BallSocketJoint(lattice[i, j], lattice[i + 1, j + 1], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
                        joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);
                    }

                    if (i + 2 < numRows && j + 2 < numColumns)
                    {
                        //Add in skipping 'starch' connections.
                        joint = new BallSocketJoint(lattice[i, j], lattice[i + 2, j], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
                        joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);

                        joint = new BallSocketJoint(lattice[i, j], lattice[i, j + 2], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
                        joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);

                        joint = new BallSocketJoint(lattice[i, j], lattice[i + 2, j + 2], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
                        joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness;
                        joint.SolverSettings.MaximumIterationCount = clothIterations;
                        Space.Add(joint);
                    }

                    //Add in collision rules.
                    if (j - 1 >= 0)
                    {
                        if (i - 1 >= 0) CollisionRules.AddRule(lattice[i, j], lattice[i - 1, j - 1], CollisionRule.NoBroadPhase);
                        CollisionRules.AddRule(lattice[i, j], lattice[i, j - 1], CollisionRule.NoBroadPhase);
                        if (i + 1 < numRows) CollisionRules.AddRule(lattice[i, j], lattice[i + 1, j - 1], CollisionRule.NoBroadPhase);
                    }

                    if (i + 1 < numRows) CollisionRules.AddRule(lattice[i, j], lattice[i + 1, j], CollisionRule.NoBroadPhase);
                }

            //Add some ground.
            var sphere = new Sphere(new Vector3(7, 0, 0), 10);
            sphere.Material.KineticFriction = .2f;
            Space.Add(sphere);
            Space.Add(new Box(new Vector3(0, -20.5f, 0), 100f, 10, 100f));

            game.Camera.Position = new Vector3(0, 5, 25);
        }
        internal void PreStep(float dt)
        {
            parentA = contact.collisionPair.ParentA;
            parentB = contact.collisionPair.ParentB;

            //Set up the jacobians.
            linearAX = contact.Normal.X;
            linearAY = contact.Normal.Y;
            linearAZ = contact.Normal.Z;
            //linearBX = -linearAX;
            //linearBY = -linearAY;
            //linearBZ = -linearAZ;

            //angular A = Ra x N
            angularAX = (contact.Ra.Y * linearAZ) - (contact.Ra.Z * linearAY);
            angularAY = (contact.Ra.Z * linearAX) - (contact.Ra.X * linearAZ);
            angularAZ = (contact.Ra.X * linearAY) - (contact.Ra.Y * linearAX);

            //Angular B = N x Rb
            angularBX = (linearAY * contact.Rb.Z) - (linearAZ * contact.Rb.Y);
            angularBY = (linearAZ * contact.Rb.X) - (linearAX * contact.Rb.Z);
            angularBZ = (linearAX * contact.Rb.Y) - (linearAY * contact.Rb.X);

            //Compute inverse effective mass matrix
            float entryA, entryB;

            //these are the transformed coordinates
            float tX, tY, tZ;
            if (parentA.isDynamic)
            {
                tX = angularAX * parentA.inertiaTensorInverse.M11 + angularAY * parentA.inertiaTensorInverse.M21 + angularAZ * parentA.inertiaTensorInverse.M31;
                tY = angularAX * parentA.inertiaTensorInverse.M12 + angularAY * parentA.inertiaTensorInverse.M22 + angularAZ * parentA.inertiaTensorInverse.M32;
                tZ = angularAX * parentA.inertiaTensorInverse.M13 + angularAY * parentA.inertiaTensorInverse.M23 + angularAZ * parentA.inertiaTensorInverse.M33;
                entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + 1 / parentA.mass;
            }
            else
                entryA = 0;

            if (parentB.isDynamic)
            {
                tX = angularBX * parentB.inertiaTensorInverse.M11 + angularBY * parentB.inertiaTensorInverse.M21 + angularBZ * parentB.inertiaTensorInverse.M31;
                tY = angularBX * parentB.inertiaTensorInverse.M12 + angularBY * parentB.inertiaTensorInverse.M22 + angularBZ * parentB.inertiaTensorInverse.M32;
                tZ = angularBX * parentB.inertiaTensorInverse.M13 + angularBY * parentB.inertiaTensorInverse.M23 + angularBZ * parentB.inertiaTensorInverse.M33;
                entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + 1 / parentB.mass;
            }
            else
                entryB = 0;

            velocityToImpulse = -1 / (entryA + entryB); //Softness?

            //Bounciness
            bias =
                MathHelper.Min(
                    MathHelper.Max(0, contact.PenetrationDepth - contact.collisionPair.allowedPenetration) *
                    contact.collisionPair.space.simulationSettings.CollisionResponse.PenetrationRecoveryStiffness / dt,
                    contact.collisionPair.space.simulationSettings.CollisionResponse.MaximumPositionCorrectionSpeed);
            if (contact.collisionPair.Bounciness > 0)
            {
                //Compute relative velocity
                float relativeVelocity = parentA.linearVelocity.X * linearAX + parentA.linearVelocity.Y * linearAY + parentA.linearVelocity.Z * linearAZ +
                                         parentA.angularVelocity.X * angularAX + parentA.angularVelocity.Y * angularAY + parentA.angularVelocity.Z * angularAZ -
                                         parentB.linearVelocity.X * linearAX - parentB.linearVelocity.Y * linearAY - parentB.linearVelocity.Z * linearAZ +
                                         parentB.angularVelocity.X * angularBX + parentB.angularVelocity.Y * angularBY + parentB.angularVelocity.Z * angularBZ;
                relativeVelocity *= -1;
                if (relativeVelocity > contact.collisionPair.space.simulationSettings.CollisionResponse.BouncinessVelocityThreshold)
                    bias = MathHelper.Max(relativeVelocity * contact.collisionPair.Bounciness, bias);
            }

            //Warm starting
#if !WINDOWS
            Vector3 linear = new Vector3();
            Vector3 angular = new Vector3();
#else
            Vector3 linear, angular;
#endif
            linear.X = accumulatedImpulse * linearAX;
            linear.Y = accumulatedImpulse * linearAY;
            linear.Z = accumulatedImpulse * linearAZ;
            if (parentA.isDynamic)
            {
                angular.X = accumulatedImpulse * angularAX;
                angular.Y = accumulatedImpulse * angularAY;
                angular.Z = accumulatedImpulse * angularAZ;
                //parentA.ApplyLinearImpulse(ref linear);
                //parentA.ApplyAngularImpulse(ref angular);
            }
            if (parentB.isDynamic)
            {
                linear.X = -linear.X;
                linear.Y = -linear.Y;
                linear.Z = -linear.Z;
                angular.X = accumulatedImpulse * angularBX;
                angular.Y = accumulatedImpulse * angularBY;
                angular.Z = accumulatedImpulse * angularBZ;
                //parentB.ApplyLinearImpulse(ref linear);
                //parentB.ApplyAngularImpulse(ref angular);
            }
        }
 ///<summary>
 /// Enqueues a change to an entity's linear velocity.
 ///</summary>
 ///<param name="entity">Entity to target.</param>
 ///<param name="newLinearVelocity">New linear velocity of the entity.</param>
 public void EnqueueLinearVelocity(Entity entity, ref System.Numerics.Vector3 newLinearVelocity)
 {
     stateChanges.Enqueue(new EntityStateChange { target = entity, vector = newLinearVelocity, targetField = TargetField.LinearVelocity });
 }
 ///<summary>
 /// Enqueues a change to an entity's position.
 ///</summary>
 ///<param name="entity">Entity to target.</param>
 ///<param name="newPosition">New position of the entity.</param>
 public void EnqueuePosition(Entity entity, ref System.Numerics.Vector3 newPosition)
 {
     stateChanges.Enqueue(new EntityStateChange { target = entity, vector = newPosition, targetField = TargetField.Position });
 }
Exemple #34
0
        /// <summary>
        /// Locates the closest support entity by performing a raycast at collected candidates.
        /// </summary>
        /// <param name="supportEntity">The closest supporting entity.</param>
        /// <param name="supportLocation">The support location where the ray hit the entity.</param>
        /// <param name="supportNormal">The normal at the surface where the ray hit the entity.</param>
        /// <param name="supportDistance">Distance from the character to the support location.</param>
        /// <returns>Whether or not a support was located.</returns>
        private bool findSupport(out object supportEntityTag, out BEPUphysics.Entities.Entity supportEntity, out Vector3 supportLocation, out Vector3 supportNormal, out float supportDistance)
        {
            supportEntity    = null;
            supportEntityTag = null;
            supportLocation  = BEPUutilities.Toolbox.NoVector;
            supportNormal    = BEPUutilities.Toolbox.NoVector;
            supportDistance  = float.MaxValue;

            const float fudgeFactor = 0.1f;
            Vector3     rayOrigin   = this.Body.Position;

            rayOrigin.Y += fudgeFactor + this.Height * -0.5f;

            for (int i = 0; i < this.collisionPairCollector.CollisionInformation.Pairs.Count; i++)
            {
                var pair = this.collisionPairCollector.CollisionInformation.Pairs[i];
                //Determine which member of the collision pair is the possible support.
                Collidable candidate = (pair.BroadPhaseOverlap.EntryA == collisionPairCollector.CollisionInformation ? pair.BroadPhaseOverlap.EntryB : pair.BroadPhaseOverlap.EntryA) as Collidable;
                //Ensure that the candidate is a valid supporting entity.
                if (candidate.CollisionRules.Personal >= CollisionRule.NoSolver)
                {
                    continue;                     //It is invalid!
                }
                if (candidate.CollisionRules.Group == Character.NoCollideGroup)
                {
                    continue;
                }

                //The maximum length is supportHeight * 2 instead of supportHeight alone because the character should be able to step downwards.
                //This acts like a sort of 'glue' to help the character stick on the ground in general.
                float maximumDistance;
                //The 'glue' effect should only occur if the character has a solid hold on the ground though.
                //Otherwise, the character is falling or sliding around uncontrollably.
                if (this.HasTraction && !this.IsSwimming)
                {
                    maximumDistance = fudgeFactor + (this.SupportHeight * 2.0f);
                }
                else
                {
                    maximumDistance = fudgeFactor + this.SupportHeight;
                }

                foreach (Vector3 rayStart in this.rayOffsets.Select(x => x + rayOrigin))
                {
                    BEPUutilities.RayHit rayHit;
                    // Fire a ray at the candidate and determine some details!
                    if (candidate.RayCast(new Ray(rayStart, Vector3.Down), maximumDistance, out rayHit))
                    {
                        Vector3 n = Vector3.Normalize(rayHit.Normal);

                        if (n.Y > supportNormal.Y)
                        {
                            supportNormal = n;
                        }

                        // We want to find the closest support, so compare it against the last closest support.
                        if (rayHit.T < supportDistance && n.Y > 0.25f)
                        {
                            supportDistance = rayHit.T - fudgeFactor;
                            supportLocation = rayHit.Location;
                            if (rayHit.T < 0.0f)
                            {
                                supportNormal = Vector3.Up;
                            }

                            var entityInfo = candidate as EntityCollidable;
                            if (entityInfo != null)
                            {
                                supportEntity    = entityInfo.Entity;
                                supportEntityTag = supportEntity != null ? supportEntity.Tag : candidate.Tag;
                            }
                            else
                            {
                                supportEntityTag = candidate.Tag;
                            }
                        }
                    }
                }
            }

            bool isSupported = supportDistance < float.MaxValue;

            return(isSupported);
        }
 /// <summary>
 /// Determines if the entity is affected by the force field.
 /// </summary>
 /// <param name="testEntity">Entity to test.</param>
 /// <returns>Whether the entity is affected.</returns>
 public override bool IsEntityAffected(Entity testEntity)
 {
     return true;
 }
 /// <summary>
 /// [Utility] Recovers the physic object from entity.
 /// </summary>
 /// <param name="entity">The entity.</param>
 /// <returns></returns>
 public static IPhysicObject RecoverIPhysicObjectFromEntity(BEPUphysics.Entities.Entity entity)
 {
     return(entity.CollisionInformation.Tag as IPhysicObject);
 }